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ABSTRACT 


The Naval Postgraduate School autonomous underwater vehicle (AUV) Phoenix did 
not have any sonar classification capabilities and only a basic collision avoidance system. 
The Phoenix also did not have the capability of dynamically representing its environment 
for path planning purposes. 

This thesis creates a sonar module that handles real-time object classification and 
enables collision avoidance at the Tactical level. The sonar module developed 
communicates directly with the available sonar and preprocesses raw data to a range/ 
bearing data pair. The module then processes the range/bearing data using parametric 
regression to form line segments. A polyhedron-building algorithm combines line 
segments to form objects and classifies them based on their attributes. When the Phoenix 
is transiting, the classifying algorithm detects collision threats and initiates collision 
avoidance procedures. 

The result of this thesis is a fully implemented sonar module on the Phoenix. This 
module was tested in a virtual world, test tank and in the first ever sea-water testing of the 
Phoenix. The sonar module has demonstrated real-time sonar classification, run-time 
collision avoidance and the ability to dynamically update the representation of the 
unknown environment. The sonar module is a forked process written in the "C" language, 
functioning at the Tactical level. Source code and output from an actual Phoenix mission 


displaying the object classification of the sonar module are included. 
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I. INTRODUCTION 


A. MOTIVATION 


Mine warfare has long been a major challenge for the United States Armed Forces. 
Current mine countermeasure (MCM) methods, which are sometimes as rudimentary as a 
sailor with an M-14 rifle, are unacceptable and need to be improved. In a recent White 
Paper "Mine Countermeasures - An Integral Part of Our Strategy and Our Forces," Chief 
of Naval Operations J. M. Boorda cited the recent damages to the USS Samuel B. Roberts 
(FFG-58), Tripoli (LPH-10), and Princeton (CG-59) to demonstrate the threat of naval 
mines (Boorda 96). The cost of the damages ($125 million) compared to the cost of the 
mines (approximately $30 thousand) demonstrates the need for better research and 
development of results in MCM. Because of the desirability of using robots to perform 
undesirable and dangerous tasks, autonomous underwater vehicles (AUVs) are an 
attractive possibility to the mine hunting problem in shallow waters. 

Many capabilities are required for an AUV to support mine hunting. At a minimum 
an AUV must be able to perform real-time sonar classification and demonstrate run-time 
collision avoidance. The abilities to detect, localize and classify unknown objects are 
essential requirements for this mission. Collision detection and collision avoidance are 
required for the safe operation of an AUV in unknown waters. 

The Naval Postgraduate School AUV (named Phoenix) is an ideal platform for 
shallow water mine hunting experiments. The current internal design of the Phoenix 15 
shown in Fig. 1.1, and an external drawing is displayed in Fig. 1.2. The Phoenix has 
demonstrated the ability to operate untethered in an unknown environment, enabling 


researchers to conduct complex experiments in mine hunting. 
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Figure 1.1. Internal View of Phoenix (Marco 96). 
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Figure 1.2. External Views of Phoenix (Marco 96). 


B. PHOENIX PROJECT 


1. Software Architecture 


The Phoenix is controlled by a tri-level architecture called the Rational Behavior 
Model (RBM) (Byrnes 93). RBM consists of a top level that is entirely symbolic with no 
global variables, a bottom level that is synchronous and numerically intensive, and a 
middle level which provides analytic modules and interfaces between the other two 
levels. The current implementation also contains some of the hardware control at the 
middle level. 

The highest level of the model is the Strategic level, this level has the mission plan 
and controls the actions of the vehicle. The Strategic level ensures that the mission is 
completed to the greatest extent possible, by passing commands to the Tactical level. 

The Tactical level is made up of numerous processes written in the "C" language, 
the main process 15 the Officer of the Deck (OOD) module. The OOD is the only module 
that communicates with the Strategic and Execution levels. The OOD forks all of the 
other Tactical level modules, the Navigator module, the Replanner module, and the Sonar 
module. The OOD then passes commands to these processes as required by the current 
phase of the mission. The OOD module also processes the data from the forked 
processes and initiates the required actions, 1.e., orders to the Execution level or responses 
to the Strategic level. 

The Execution level directly controls the hardware based on the orders from the 
Tactical level. Real-time constraints required for the stability of the AUV are all handled 
at this level. Many safety features are also built into this level that might cause a fail-safe 
abort to the surface. The automatic abort situations include flooding, loss of 
communications with the Tactical level, loss of DiveTracker acoustic navigation and loss 
of depth control. 

Pipes are used for interprocess communication (IPC) on the Tactical level. 


Communication between the Strategic level and Tactical level is done with function calls 


and returned values. Communication between the Execution level and Tactical level is 
accomplished using sockets. The Sonar module and the Navigator module communicate 
with their respective sensor hardware through serial ports. A communications diagram 


appears in Fig 2.1. 


2. Hardware Systems 


The current hardware in the vehicle consists of a Gespac M68030 series computer 
system and a Sun Voyager Sparc 5 workstation. The OS-9 operating system is used on 
the Gespac providing the real-time multitasking needed by the Execution level for 
hydrodynamic control stability. The Tactical and Strategic levels run on the Sun Voyager 
under SunOS 5.4. The two computer systems form a LAN through Ethernet connections 
within the vehicle, which can also be (optionally) networked through an external Ethernet 
connection. Communications between systems is done through software sockets. The 
Sonar and Navigator modules communicate with peripherals through serial ports, one of 
which is used directly by the Voyager. Other serial ports connect through a SCSI serial 
interface which increases the number of remaining serial ports available for future use. 

The available sonar systems are a Tritech ST725, which is a 750 kHz scanning 
sonar and a Tritech ST1000, a 1250 kHz profiling sonar [Tritech 92]. The ST725 has a 
one degree wide by 24 degree vertical fan beam. This beam is steerable with azimuth 
rotation step sizes of 0.9, 1.8 and 2.6 degrees. The range options for the ST725 are one, 
two, four, six, ten, 20, 25, 30, 50 and 100 meters. The ST1000 sonar can operate both in 
scanning mode and profiling mode. The ST1000 transmits a one degree conical beam. 
The range scale consists of eight possible selections ranging from three to 160 feet. Both 
sonar systems are steerable, thus providing 360 degree coverage. Some "baffling" 

(i.e. occlusion) of sonar signals is possible when pointing astern due to returns from the 


vehicle. 


C. THESIS OBJECTIVES 


The objective of this work was to create a Sonar module that operates at the 
Tactical level where it receives orders from the OOD module and conducts sonar searches 
based on those orders. This module communicates directly with the sonar systems to 
collect raw data, processes the raw data to perform real-time sonar classification and 
produces the messages required to achieve run-time collision avoidance. To achieve 
these goals, the information developed by the Sonar module in the form of "new worlds" 
(i.e. circle models) and "collision threats" is passed to the OOD to initiate the required 
actions. A description of Phoenix sonar operations follows. 

The modes of operation now available for Sonar are "Transit Search," "Sonar 
Search" and "Rotate Search." Transit Search is performed when the Phoenix is transiting 
between waypoints, continuously scanning the sonar between 325 and 035 degrees 
relative bearing. The main purpose of Transit Search is collision avoidance. The Sonar 
Search and Rotate Search are used when the Phoenix is stationary and a search of the 
surrounding area is desired. These searches are used to locate and classify unknown 
objects. The Sonar Search is a 360 degree rotation of the sonar with the vehicle heading 
fixed, while the Rotate Search is done with the sonar head fixed and a 360 degree rotation 
of the vehicle. 

Sonar classification begins with the preprocessing of the raw sonar data The 
resulting processed returns are then fitted to line segments using parametric regression. 
Line segments are then combined based on proximity and orientation to form polyhedra. 
The polyhedra are classified based on their characteristics. The classified objects are then 
represented as circles in a world file, which is then used by the Replanner to plan the 
paths between waypoints. 

Collision avoidance is accomplished by evaluating the range and bearing of each 
sonar return during the building of line segments. When collision threats are discovered 


they are passed to the OOD, permitting collision avoidance actions to be taken. The 


current execution level collision avoidance actions, which merely backs down until 


headway is removed, can easily be improved to take less drastic measures. 


D. SUMMARY 


The recent losses the United States Navy has experienced due to mine warfare has 
shown the urgent need to improve MCM. AUVs are one of the potential platforms that 
could be used for mine-hunting in very shallow water. The Naval Postgraduate School 
Phoenix AUV has progressed to the point where it 1s an ideal platform for AUV 
experiments in shallow-water mine-hunting. 

The tri-level architecture of the Phoenix can be compared to the human chain of 
command on board a real submarine with the Strategic level corresponding to the 
Commanding Officer (CO), the Tactical level corresponding to the supervisory watches, 
and the Execution level corresponding to the watchstanders in the spaces who operate the 
equipment. The Sonar module create by this work represents the sonar watchstanders 
who analyze the raw data and the sonar supervisor who evaluates their results and reports 


to the OOD. 





II. PREVIOUS WORK 


A. INTRODUCTION 


Previous work on the Phoenix project has created a software architecture paradigm 
called the Rational Behavior Model (RBM) (Byrnes 93). Other previous work related to 
the Phoenix project includes an expert system for sonar classification (Brutzman 92) and 
a circle world replanner (Brutzman 92). All of these works have been improved and 
reimplemented to advance the Phoenix project. The virtual world created by 
(Brutzman 94) also played a vital role in the development of the software modules for the 


Phoenix. 


B. TRI-LEVEL ARCHITECTURE 


]. Paradigm 


The current implementation of the RBM is represented in Fig. 2.1. This control 
architecture resembles the command structure of a manned submarine. The strategic 
level corresponds to the Commanding Officer (CO), the tactical level corresponds to the 
OOD and watch officers, and the execution level corresponds to the actual equipment and 


watchstanders operating the equipment. 


2. Strategic Level 


This level controls the overall condition of the vehicle through planning and 
deciding on operational tasks, and then giving orders to the OOD. These orders are based 
on the current phase of the mission. Responses from the Tactical level are evaluated to 
determine the status for the current phase, as well as determining whether to proceed to 
another phase or to abort the mission. The Strategic level is currently implemented in 


Prolog. 
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Figure 2.1 Current Software Architecture and Communication Modes. 


3. Tactical Level 


This level is run by the OOD module, which forks the other modules at startup. 
The OOD is the only module that communicates with the other levels, via sockets with 
the Execution level and via function calls and returned values with the Strategic level. 
All communications within the Tactical level are done via pipes. Since there is no shared 
memory established in the current implementation, all data that 1s required by more than 
one module is either passed in message format or written to files to be accessed at a later 
time by other modules. The modules that are currently operating at this level are the 
OOD module, the Sonar module, the Navigator module, and the Replanner module. All 


processes in this level are implemented in the "C" language. 
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4. Execution Level 


The Execution level alone resides on the Gespac system. This level is currently 
implemented in the "C" language and is written to work both onboard the Phoenix and in 
the virtual world. The Execution level handles the control and stability of the Phoenix 
through the control of all of the control planes, thrusters, and screws. This level also 
implements many safety features to perform an abort script that aborts the mission by 
surfacing the AUV. This abort script can be triggered by low battery voltage, flooding, 


loss of depth control, or loss of tactical level communications. 


C. EXPERT SYSTEM FOR SONAR CLASSIFICATION 


Previous work developed an expert system for sonar classification (Brutzman 92). 
The sonar classification expert system was written in the Clips language (NASA 91) and 
processed sonar data offline due to the computational demands of the expert system. The 
starting point of this thesis was to convert that expert system into an onboard real-time 
system. The existing expert system uses parametric regression to line fit the sonar data. 
A sliding window is used to locate a suitable starting point for a line segment. Line 
segments are combined based on time sequence, distance and orientation. The combined 


line segments build polyhedra which are then classified based on their characteristics. 


D. CIRCLE WORLD REPLANNER 


Current work has developed a Replanner module at the Tactical level 
(Leonhardt 96) . This module creates a safe path between the AUV's current location and 
the desired location, using the "circle world.inputX" file created by the Sonar module. 
The input file has the format of "Circle x-position y-position Z-position radius." This 
module was derived from the work of (Brutzman 92). The Replanner uses the circle 
world representation of the obstacles, the start point, and the goal point to create a file 


containing segments and arcs which can be used to safely traverse the obstacle field. 
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E. VIRTUAL WORLD 


The virtual world created by (Brutzman 94) is an invaluable asset in the 
development of new software for the Phoenix. The virtual world allows for initial testing 


of new software and software modifications without the vehicle being deployed in water. 


F. CURRENT WORK 


The Phoenix project has made significant advancements during the past six months. 
Many of these improvements are discussed in this thesis and (Leonhardt 96). Other 
recent work includes the combination of the virtual world's and Phoenix's Execution 
levels to form a single execution program that works in both environments (Burns 96). 
The installation of DiveTracker acoustic navigation on the Phoenix was part of the work 
by (Scrivener 96), the integration of the DiveTracker and the GPS data into the Navigator 


module was accomplished by (McClarin 96). 


G. SUMMARY 


The use of autonomous vehicles for jobs that are either undesirable or dangerous 
for human beings is the driving force behind many robotics research projects. MCM is an 
area that is ideal for robots. Many organizations have been working on the issue of 
autonomy for robots for many years. With the work of this thesis and the many works 
cited above, the Phoenix has demonstrated the ability to operate in, and interact with, an 
unknown environment. These capabilities allow for further testing and software 
development in the support of mine-hunting solutions. It is now clear that AUVs are on 


the threshold of effectively performing minefield search missions. 


III. PROBLEM STATEMENT 


A. INTRODUCTION 


Real time sonar classification and run time collision avoidance are mandatory 
requirements for truly autonomous operation in unknown environments. To achieve 
these objectives, the process starts with the gathering and processing of raw sonar data, 
including the initialization the sonar system. The next problem is to create object 
representations from the processed sonar data using polyhedra. Once the polyhedra are 
built, object classification occurs. The final step in the classification process is the 


representation of the objects in a format that allows for path planning. The flow of data is 
shown in Fig. 3.1. 


Raw Sonar Data 


Sonar Pre-Processing 


Range & Bearing 


World Coordinate 
Transformation 


| X & Y position 


Parametric Line Fitting 


Polyhedron Building 


Polyhedron 


Object Classification 


Object Representation For Path Planning 





Figure 3.1 Sonar Classification Data Flow Chart. 


15 


The collision avoidance task is essentially independent of sonar classification. 
Collision avoidance can be broken into two problems: first identifying the existence of 


collision threat, and second reacting both properly and promptly to avoid a collision. 


B. SONAR DATA PROCESSING 


1. ST725 Scanning Sonar 


Sonar processing begins with the initialization of the sonar head. This step is where 
the maximum range, azimuth rotation step size, receiver gain, and transmitter power 
parameters are set. The second step in the process is to analyze the data returned by the 
sonar head. The ST725 sonar returns a 33 byte string, with 32 bytes of the string 
representing the strength (between zero and fifteen) of the sonar return over the range 
scale divided into 64 bins. The preprocessing of this ping return data must produce a 


single range/bearing pair, to be further analyzed by the classification algorithm. 


2. ST1000 Profiling Sonar 


The ST1000 sonar can be operated as a scanning sonar, like the ST725, with 64 or 
128 bins thus presenting the same preprocessing requirements that the ST725 sonar 
presents. The ST1000 can also be operated in profiling mode, where the return is a range 
in mm. ST1000 profiling mode does not require range/bearing postprocessing as the 


scanning mode does. 


C. REAL-TIME SONAR CLASSIFICATION 


l. Line Fitting 


The first step of the classification problem begins with fitting the processed sonar 
data into line segments. The line fitting problem consists of starting the line segment, 
adding to the line segment once started, and when to finish the line segment. The ability 
to handle spurious and intermittent returns is a problem that also needs to be addressed 


while fitting lines to the data. 
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2. Polyhedron Building 


Once line segments are formed the problem becomes how and when to combine the 
line segments to build objects. Our approach is to first create cylindrical polyhedra. The 
storage of objects once they have been formed, presents problems regarding what data 


structures are needed and what information needs to be maintained? 


3. Classification of Objects 


The final step in the process is the actual classification of the object. This step 
involves determination of what characteristics should be used for classification, and how 
the characteristics will be used for classification. Determining when an object should be 
classified 1s also an important issue: should the object be classified during the building 


process or only when an object is completed. 


4. Representation of Obstacles 


The final step in the classification process is how to represent the objects for path 
planning purposes. This inciudes the problem of how to format this information and how 


to share it with the OOD to support autonomous path planning. 


О. OBSTACLE AVOIDANCE 


1. When to Check for Collision Threats 


The first question in obstacle avoidance 15 to determine when to check for a threat. 
Should a check be done for every valid return, when a return contributes to a line 
segment, or when a line segment is ended? We investigate this question and provide a 


workable initial approach. 


2. Identification of a Collision Threat 


The next step in the obstacle avoidance problem is the determination of the 
existence of a collision threat. This process begins with the declaration of what 
constitutes a collision threat and then the ability to recognize it from the sonar data at run 
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time. An important criterion in the identification process is present (and intended) motion 


of the AUV. 


3. Collision Avoidance Actions 


With successful identification of a collision threat the next issue becomes deciding 
what actions need to be taken and when they must be initiated. Such actions must take 


into account the current phase of the mission. 


E. SUMMARY 


This chapter summarizes the problems addressed by this thesis. Real-time sonar 
classification and run-time collision avoidance are critical parts of autonomous 
operations. To achieve these features many problems must be solved. After the 
initialization of the sonar systems, preprocessing of raw sonar data must be performed. 
Then the real-time sonar classification problem is addressed by line fitting, polyhedron 
building, and object classification. The problem of obstacle avoidance includes 
determining what is a collision threat, when to check for a collision threat, and how to 


respond to a collision threat. 
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IV. THEORETICAL DEVELOPMENT 


A. INTRODUCTION 


This chapter examines the real-time sonar classification problem in detail. The 
sonar classification process begins with the initialization of the sonar system, and then the 
gathering and preprocessing of raw sonar data. Range/bearing data is then fitted to line 
segments using parametric regression. The polyhedron-building algorithm then takes the 
line segments and combines them to form a polyhedron representation of the underwater 
objects that caused the sonar returns. Object classification 1s done based on the attributes 
of the polyhedron. 

The collision avoidance problem is solved in two steps: first the ability to detect a 
collision threat, and second the ability to react in time to avoid the collision. The ability 
to react in a timely fashion results in the requirement of collision threat evaluation at a 
much higher frequency than object classification. 

The final stage of the classification process 15 (ће representation of the classified 
objects for path planning purposes. A solution to the path planning problem is 
demonstrated in (Leonhardt 96) using circle representations of obstacles, that are the 


product of the sonar classification process. 


B. SONAR DATA 


1. Initialization of Sonars 


The parameters of the sonar to be set at the initialization phase are, maximum 
range, receiver gain, azimuth change step size, transmitter power, mode (ST1000 only), 
and numbers of bins (ST1000 only). The initial settings are based on knowledge of the 
operating area. The initialization of the sonar system also requires the initialization of the 


serial port that is to be used for communications. The serial port used is "/dev/ttya" on 
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the Sun Voyager. The initialization that has to occur for the serial port every time the 
Voyager is rebooted is as follows: 

* Become Super User to gain necessary permissions 

¢ cd /opt/CDsts 


¢  /cdsoftcar -y /dev/ttya 


2. Gathering of Raw Data 


Sonar data can be collected using the AUV's ST725 scanning sonar or the ST1000 
profiling sonar. The sonar used is based on which type of sonar search is chosen from the 
three types of sonar searches that can be conducted. The first sonar search is the Transit 
search. As its name implies, this search is conducted when the AUV is transiting 
between points. The Transit search is a back-and-forth scanning search between relative 
bearings 320 degrees and 040 degrees. The other two searches are complete 360 degree 
searches, used to conduct a thorough search of an unknown area. These two searches are 
the Sonar search where the sonar is scanned 360 degrees and, the Rotate search where the 


АЗУ is rotated 360 degrees. 


3. ST725 Scanning Sonar 


The ST725 sonar is primarily used for the Transit search. The raw data from the 
ST725 data is received as a 256 bit string representing 64 four bit values. Thresholding, 
filtering and smoothing techniques were evaluated on raw data to determine the best 
algorithm for preprocessing. 

Our present algorithm employs a nearest-strong-return criterion, where a farther 
return on the same bearing has to be greater than one level higher to override a nearer 
return. This prevents a close weak target from being masked by a farther strong target. A 
thresholding limit of eight was set for the initial sea-water experiments at Moss Landing 
Harbor. This threshold is based on preliminary sonar testing results and likely needs to be 
evaluated on a case-by-case basis dependent on the local sonar environment. Previous 


returns are not used to filter spurious data, since a large distant target might mask a small 
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closer target. Instead spurious returns are identified and rejected at the parametric 
regression level. Depending on the range scale used, some of the initial bins are ignored 
as self noise. The number of bins ignored is a function of the relative bearing of the sonar 
in order to reduce "baffling" (i.e. self-occlusion) problems. The output of the sonar 
preprocessing algorithm is a single range/bearing data pair. 

Figure 4.1 shows example raw sonar data from Moss Landing. Corresponding 
output of the sonar preprocessing algorithm is shown in Fig. 4.2, demonstrating the 
efficiency of the preprocessing algorithm. Sonar returns in the forward port quadrant are 


from a pier. 





Figure 4.1. Raw Sonar Data Unprocessed. Returns in the 
Forward Port Quadrant are From a Pier. 


4. ST1000 Profiling Sonar 


The ST1000 sonar can operate both in a scanning mode where all of the 


preprocessing is the same as for the ST725, or in a profiling mode where the return is a 
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range in mm. Mode of operation is based on the initialization of the sonar head. The 
profiling mode provides more accuracy and requires less processing of the sonar data. 


Currently profiling mode is used during sector searches to take advantage of the superior 


range accuracy. 
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Figure 4.2. Preprocessed Sonar Data from Moss Landing 
Using a Data Set Similar to that Shown in Fig. 4.1 


5. Coordinate Transformation 


The output from the sonar processing algorithm is a relative range and bearing. The 
range and bearing data is composed with the actual position and orientation of the 
Phoenix AUV, to determine x return, and y. return, which are then used by the line fitting 
algorithm. The transformation to world coordinates is shown in Fig. 4.3. 

Two-dimensional coordinate transformations are shown in Equations (4.1) and 
(4.2). The x and y values represent the center of buoyancy coordinates of the AUV, 
provided by the Execution level. The sonar offset from the AUV center is three feet. The 
translation to the actual sonar is done by the 3*sin(AU Vheading) portion of the equation, 


with the units of feet. 
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Figure 4.3. World Coordinate Transformation. 


x return — x * 3 * cos (AUVheading) + range * cos (AUVheading + bearing) (Eq. 4.1) 
y return —^ y * 3 * sin (AUVheading) ^ range * sin (AUVheading + bearing) (Eq. 4.2) 


The effect of roll and pitch to the sonars is ordinarily small and is ignored due to the 
small errors of the AUV on these axes. The preprocessing of sonar data is independent of 
any motion by the Phoenix, due to the frequent (six to ten Hz) position updates provided 
by the execution level. The execution level dead reckons (estimates) the position using 
heading and speed. AUV speed is determined using a mathematical model at low speeds 


(less than one knot) and speed wheel sensor at higher speeds. The position is periodically 
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reset by the navigator module. The navigator module uses a kalman filter with inputs 


from DiveTracker and GPS. 


C. LINE FITTING USING PARAMETRIC REGRESSION 


1. Parametric Regression 


The usual method of linear fitting 1s using a least-squares fitting algorithm in 
Cartesian coordinates. An unfortunate problem with this method is that it falls apart 
when data points are parallel to the y-axıs, producing lines with infinite slope and 
resulting in a divide by zero situation (Kanayama 95). This problem has been eliminated 
by reformulation of least-squares line fitting using parametric representations of lines. 
The parametric approach is suited for real-time applications, due to its sequential 
incremental characteristics which can provide usable results at any time. A derivation of 
the algorithm follows. Further detail on the parametric regression line-fitting algorithm 
can be found in (Kanayama 95). 


Given a set R of sonar data points: 


R 2 (xi, yp i 2 1,..., n) . (Eq 4.3) 
The moments of R are defined as 


n 
ту = 2, ду (0€ j,k €2,and j - k € 2) (Eq 4.4) 


iz] 
Notice that moo= n. The centroid C of R is given by 


C= Cag moo) кита y (Eq 4.5) 


The secondary moments about the centroid are given by 
n 


M9 = D(x) — их)? = то — (то)? (Ед 4-6) 
pi 
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п 





Ми = Убх р) у) = ти – (по) (Ед 4.7) 
= | 
n 
Mq 7 3i - uy? 2 moa - Ges)” (Eq 4.8) 
[= 1 


The parametric representation of a line is adopted, with constants r and ©. If a 


point p = (x, y) satisfies the equation 
г=хсоѕ 2+ уѕіпо (7/2 < 0 < 1/2) (Ед. 4.9) 


then the point p is on a line L whose normal has an orientation œ and whose distance 
from the origin is r shown in Fig. 4.4. In the parametric representation, the signed 
distance from a point p; = (xj, y;), to the line L = (r, œ), is called the residual (5), and 
calculated as follows. 


б; = х; * соѕ (0) + у; * ѕіп (0) – /. (Ед. 4.10) 


Therefore the sum of the squares of all of the residuals is 


n 
S 2 Mr - xjcos a — у;ѕіпо)2. (Eq 4.11) 
= 
The best line fit of the set of data points will minimize S. The optimum line (r,o ), must 


satisfy 


5718 
| 
24/2 
| 
О 


(Eq 4.12) 
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Figure 4.4. Representation of a Line. 6 is the Residual from a Point. 


Thus, 
n 
= = 2 » (r — xjcos € — y;sin a) 
=! 
п п 4 
-2(r $,1— ($,xi)cosa - (> yi)sin a) 
= 2(ктоо - т10С08 6С— то sin (X) 
=0 
апа 
_ M10 то : = і 
r= mg COS € * gg S1n = HxCOS Q + ysin Q 


where r may be negative. Substituting r in (Eq 4.11) by (Eq 4.14), 


n 


S = У ((х; – Цајсов а: + (у; – ну) 002. 
= | 


Finally, 


n 
В = 2 $ (Gri - x)cos o 4 (y; 7 у) 0) (—(х; — Hx)sin & + (y; — Цу)сов 0) 
ге 
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(Еа 4.13) 


(Ед 4.14) 


(Ед 4.15) 


(Ед 4.16) 


n 
=2 > (у; —Ну)“ - (xi -Hx)*)sin acosa + 
[= 1 
п 
2 Ух: – ц) (у: = y)Y(cos?a.- sin^a) 


E 
= (Мо> — M59)sin 26 - 2M сО5 20 


— 0 for a perfect line fit. 


Therefore 
а аап2(—2М 1 ,Mg5-M 59) 


1 (Eq 4.17) 


Here atan2 is the modified arctangent function which returns an angle in the proper 
quadrant. The solutions for the line parameters generated by a least-squares fit are given 
by Equations (4.14) and (4.17). 


The equivalent ellipse of inertia for the original n points is an ellipse which has the 


same moments around the center of gravity. M major and M minor are moments about the 


major and minor axes respectively, shown in Fig. 4.5. 


M major = (M20 + Mo2)/2 — (Moo — Ma29))/4 Мт (Eq 4.18) 


M minor = (M20 + Mo2)/2 + | (Мо = М2о)?/4 + М1. (Ед 4.19) 


The diameters dmajo: on the major axis and dino, On the minor axis of the equivalent 


A minor = 4. | М пајог!то0 (Ед 4.20) 


А пајот = 44 M minor! oo (Eq 4.21) 


ellipse are 
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We define p, the ellipse thinness ratio, to be the ratio Of dmino, and djngjor : 


d 


minor 


р = (Ед 4.22) 


d major 


A small p (near zero) means a thin ellipse. As p increases toward 1, the ellipse opens to 


a circle representing a non-linear set of points. For this reason, р, 1$ used as a testing 


parameter for linearity checks. 


North (x. world) 


East (y. world) 





Figure 4.5. Equivalent Ellipse of Inertia. 


2. Representation of Line Segments 


The variables maintained during the building of a line segment are enumerated in 
Fig. 4.6. The sequential nature of the parametric regression algorithm is supported by 
consistently maintaining the moments and secondary moments. These summations are 
then updated every time a point is added to the line, keeping computational complexity 


O(1) rather than O(N) while adding points. 


3. Starting Line Segments 


The previously existing expert system required five data points to begin a line 
segment. A suitable set was found using a sliding window. This was the how the sonar 
module also started line segments in the early stages. However, once data was collected 
in an actual sea water environment, it was discovered that starting with four data points 


provided better results. This window is large enough that spurious returns will not start a 
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line segment, and small enough to detect far objects that will not produce that many 
returns. 

The sliding window is implemented using two four-element arrays: initx and inity, 
as shown in Fig. 4.6. These arrays are filled with the four most recent valid sonar returns. 
A line is fitted to the data using Equations (4.14) and (4.17). If the thinness of the line is 
satisfactory (less than 0.1 for this implementation) then a segment is started. If the 
proposed line by the sliding window does not meet validity requirements the oldest point 
is thrown out and the next return is added. This process is repeated until the start of a 


valid line segment is found. 


4. Building Line Segments 


To reduce attempts to add the current sonar return to the current line segment when 
it does not fit, filtering of the sonar returns is performed. This filtering can often detect 
the end of a line segment without performing the computations necessary to include it 
into the current line and then ending it. The filtering consists of comparing the residual, 


Equation (4.10), to an maximum allowable distance from the line and comparing the 


residual to an maximum weighting of the standard deviation, o which is calculated: 


o = |M jor! (n — 1) (Eq. 4.23) 


The comparison is 


6 « max(Cl * o, C). (Eq. 4.24) 


If the point is within these parameters it is then added to the line, and the thinness ratio is 
checked for the new line segment, if the line segment has exceeded the allowable 
thinness, the line segment is ended, and the current point is stored for the next line 


segment. If the line remains acceptable all of the line segment parameters are updated. 
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typedef struct 

{ 
double theta; 
double r; 
int num_points; 
int line. status; 
double initx [4 ].inity[4 |; 
double sgm delta sq; 
double start time; 
double startx,starty; 
double endx,endy; 
double sgmx,sgmy; 
double sgmx2,sgmy2; 
double sgmxy; 
double d minor,d major; 

) SEG. DAT; 





Figure 4.6. Segment Building Data Structure. 


5. Ending Line Segments 


A line segment needs to be ended when the latest data point no longer forms an 
acceptable line, or when no sonar return is received for five seconds. Ending a line due to 
an unacceptable data point is determined during the preceding section on building line 
segments. A line segment is also be ended when the distance between the current return 
and the last return added to the line is not within an acceptable range (two feet for this 
implementation). When ending a line segment the final calculations are performed to end 
the line segment. When a line segment is ended the line parameters are calculated a final 
tme and stored in a LINE SEG structure. One parameter that is only calculated at the 


ending of a line segment is the orientation of the line, since the orientation is not used for 
building a line segment and only for the combination of line segments. The orientation O 


is calculated using the atan2 function, O — atan2(y. end — y. start, x end — x. start). The 


need for consistency in the orientation calculation regardless of the scanning direction of 


the sonar can be seen in Fig. 4.7. Where the scan in the clockwise direction (Scan B) 
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would determine an orientation of 45 degrees, and the scan in the counter clockwise 


direction (Scan A) would calculate an orientation of -135 degrees. 





Figure 4.7. Scanning Direction and Line Orientation. 


The scanning direction is not the only possible problem as shown in Fig. 4.8, AUVs 
scanning in the same direction still may not produce the same results with a simple 
atan2(endy - starty, endx - startx) calculation. For this example AUV scan "A" would 
produce an orientation equal to -135 degrees, while AUV scan "B" would calculate 45 
degrees for the same segment, even though both AUVs are scanning in the clockwise 


direction. 





Figure 4.8. Orientation Calculation from Opposite Sides of a Line Segment. 
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To provide the necessary consistency for the orientation comparison, the œ of the 
line segment is used. The issue to take into consideration when using this comparison is 


that the value of r may be negative. When adjacent segments have opposing r values, 


both r and © are negated for the second segment to permit proper comparison. 


D. BUILDING OBJECTS FROM LINE SEGMENTS 


1. Underwater Objects: Convex not Concave 


Underwater objects of interest have predominantly convex shape. This fact is the 
basis for the adjacent line combination algorithm used. If an object does have concave 
features, it is instead represented as more than one object. Since concave objects are 
ordinarily not of concern, and since the algorithm always produces a valid world 
representation of convex and concave objects for path planning purposes, this is an 


acceptable approach. 


2. Object Building 


Segments are combined to form polyhedron representations of objects. The first 
condition checked to determine whether or not to combine segments is the distance 
between the line segments. The distance comparison is done between the end points of 
one line segment to the end points of the follow-on line segment, for both possibilities 
this allows for out-of-order combination. The comparison using the most current line 
segment's start point and the previous line segments end point is done first, as this 1s the 
most likely combination. 

If the distance between the line segments is less than the permitted maximum 
tolerance the next comparison is orientation of the line segments. Alternatively normals 
may be used for comparison instead of line orientations. If the line segments are adjacent 
and colinear, then they are grouped together. The tolerance of the colinear check is 
relaxed the closer the line segments are to each other, this is due to the fact that the closer 


the line segments are the increased probability that they belong to the same object. 
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If the segments are not colinear it is then determined whether they are convex or 
concave. Due to the characteristics of underwater objects the segments are grouped 
together if they are convex and are not grouped together if they are concave. The 
problem of a consistent comparison between line segments occurs due to the possible 
change in sonar scanning direction, as well as AUV heading and bearing rate. A 
consistent comparison is needed that is independent of scan direction and relative 


position, as shown in Fig. 4.9. 


Convex 


Concave 





Figure 4.9. Convex versus Concave. 


To solve this problem the bearings of the line segments relative to the AUV are 
calculated and compared to determine the line segment which is more clockwise. The 


clockwise line segment is then treated as the second line segment in the comparison, 


(t — Q2. If the result of that comparison is negative, the segments are convex with respect 


to the AUV. 


E. CLASSIFICATION 


1. Check If New Object 


The initial part of classification is to ensure that the new object has not already been 
represented in the world. This is accomplished by comparing the classification of the 
objects and the centroids, if the classification is a wall. If the classification is a mine the 
test compares the areas of the objects as well as the centroids. The comparison is done 
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between the current object and all of the objects already defined in the world. More work 
is needed here. The first area to address would be to combine overlapping polyhedra. 
This can occur when the Phoenix AUV moves to another position relative to the target as 


demonstrated in Fig. 4.10. One way to combine polyhedra might be to calculate weighted 


averages of centroids and radii as shown in Fig. 4.11. 


/ 


Polyhedron tl == Time tl 


Polyhedron t2 — 





Figure 4.10 Overlapping Polyhedra. 


Гпем = Г] + Г2 





Figure 4.11. Merging Overlapping Circles. 
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2. Sequential Rule Firing 


Classification 1s done based on the strength of returns, linearity and area of the 
polyhedra, as well as any known characteristics of the environment. All new polyhedra 
are tested to see if they are already represented in the world model. A check for a 
possible moving target looks for objects that are identical but have traveled at a finite 


speed. Much more work is possible here. 


F. REPRESENTATION OF CLASSIFIED OBJECTS 


1. Method of Representation 


Representation of the objects is achieved using circle representations for path 
planning purposes. This representation is valid as most objects in the underwater 
environment can be adequately approximated by individual cylinders or walls of 


cylinders. 


2. Representation of Linear Objects (Walls) 


The Replanner module uses circle representation of objects for path planning. 
Since the replanner uses circles, linear objects need to be represented as circles. A 
predetermined radius is used to create circle representations of the linear object. This is 


done as shown in Fig. 4.12. 


/* split a wall into circles of global radius */ 
number of circles 7 ceiling(length/global radius); 
delta x 2 (tailx - headx) / number. of circles; 
delta y = (taily - heady) / number. of circles; 
for (12 0; 1 < number. of circles; ++1) 


( 


fprintf(new_circle_ptr,"Circle %6.4f %6.4f %6.4f %6.4f\n", 
(headx + 1*delta_x),(heady + 1*delta_y), z ,global_radius); 





Figure 4.12. Circle Representation of Linear Objects. 
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3. Representation of Polyhedra 


The area of a polyhedron is the summation of the triangle areas shown in Fig. 4.13. 


The area of a single planar triangle is given by Equation (4.25). 
Areas 7 5X2 - XiYYa - Yi) – (Хз - ХО) (У: – 1) (Eq 4.25) 


The polyhedron is represented using centroid_x and centroid_y shown in Equations 


(4.26) and (4.27). The radius of the circle is calculated in Equation (4.28). 


У(яат_х+епа_х) 


Е 


(Ед 4.26) 


>Astart_y+end_y) (Eq 4.27) 


2*number_of_line_segmei 


Centroid_y = 





Figure 4.13. Summing Triangle Areas to Determine Polyhedra Area. 


radius = [2 * Área polyhedron! TC (Eq 4.28) 


The area is multiplied by two for a safety range from the polyhedron. 
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G. COLLISION THREATS 


1. When to Check? 


The issue of collision avoidance is an important safety issue of the AUV. To 
ensure the safety of the AUV the frequency of checks must be often enough to guarantee 
a collision threat will not be missed. The elimination of the unnecessary processing of 
Spurious returns is also an issue of concern. Both problems are handled by the collision 
threat check, which is performed for every sonar return that contributes to a line segment. 
If a return does not contribute to a line segment then the return is considered spurious. 
Further work will be needed to discriminate between spurious returns and objects which 


do not provide consistent returns. 


2. What is a Collision Threat? 


A collision threat is any object that lies in the path of the AUV within a five foot 
range 1.e., a little more than one-half ship length. The safe width that is required for the 
AUV is four feet. With the required four foot width for passage and five foot safety 
range, the relative bearings that are checked for collision threats are from 336 degrees to 
024 degrees, as shown in Fig. 4.14. This simple check will detect most problems when 


transiting between waypoints. 


24 degrees 





Figure 4.14. Collision Avoidance Safety Range. 
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3. Collision Avoidance Actions 


When a collision threat is detected the sonar module passes the message 
"COLLISION THREAT' to the OOD module. The OOD module then orders a full 
backing bell until all headway is removed from the AUV. Future implementations should 
include a "collision warning" message, when an object is in the path of the AUV, but not 
close enough to be a collision threat, since this warning would allow for less drastic 
measures and easier recovery. Other actions will need to be developed for hovering 
mode. The best approach is probably to stop, hover in place, back away as necessary to 


avoid collision, map the new collision threat, and replan the path. 


H. SUMMARY 


The algorithms above have been implemented in the current sonar module 
operating on the Phoenix. The sonar module initializes the sonar system at startup. The 
run-time processing of the module includes the gathering, processing and transformation 
of the raw sonar data. The data is then fitted to line segments using parametric 
regression. The implementation of the object building and classification algorithms have 
demonstrated the ability to provide the required data for the autonomous operation of an 
underwater vehicle. Collision avoidance implementation has also been supported with 


message passing to the OOD when a collision threat occurs. 
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V. EXPERIMENTAL DESIGN AND RESULTS 


A. INTRODUCTION 


The goal for the Phoenix was to conduct a successful sea water mission of 
detecting, localizing and classifying a mine-like object. Sonar code developments were 
first tested in the virtual world, then the test tank and finally a larger mission 


demonstrating the AUV's capabilities. 


B. VIRTUAL WORLD TESTING 


1. Using the Virtual World 


A virtual world has been used throughout the development of this code 
(Brutzman 94). The virtual world allows the user to run all vehicle software verbatim, 
testing interprocess communications and algorithm correctness. While the virtual world 

allows for testing correctness, it does not test for hardware robustness, and is currently 


somewhat of an ideal environment even with sensor errors inserted. 


2. Experiments 


The initial virtual world testing was a simple mission with the AUV in the center of 
the test tank. A 360 degree rotation was performed to gather sonar data and test the 
classification algorithms. The resulting raw sonar data is shown in Fig. 5.1. The line 
segments formed from the sonar data and the circle representations of those line segments 
are Shown in Fig. 5.2. The test was done once with no knowledge of the "world." The 
initial run produced an output file called "new. world," this file is the representation of the 
environment computed by the program. The second test uses the output from the first run 
as input for the "world." This was to test the correctness of the "check if new" function. 
The ability to compare objects with the known environment reduces the communication 
between the OOD and the Sonar module, as well as an unnecessary collision threat 
reports. The results of the testing were satisfactory. 
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3. Test results 


Figure 5.1 shows the virtual world range/bearing data. Figure 5.2 shows the line 


segments fitted to the data and the center of the circles produced for path planning 


purposes. 
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Figure 5.1 Sonar Data from Virtual World. 
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Figure 5.2 Fitted Line Segments and Centers of Circle Representations. 
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C. TANK TESTING 


]. Preprocessing Real Sonar Data 


None of the computer hardware used in this thesis was installed or connected prior 
to this work. The test tank was a useful environment at first to allow for communication 
between the Voyager computer and the sonar systems. The test tank was the first 
opportunity to evaluate raw sonar data and determine the best methods for preprocessing. 
The sonar data collected was processed through various thresholding, smoothing, and 
filtering algorithms. The final algorithm developed for the sonar preprocessing is 
represented in Fig. 5.3. The input to the algorithm is a 64-bin range array, with each bin 
containing a number between zero and 15 representing the average strength of the return 
over that portion of the range scale. The first step of the algorithm is bin thresholding, 
which is the process of ignoring some of the initial bins to eliminate interference from 
self noise. The number of bins that are ignored completely is based on the relative 
bearing of the sonar, in order to eliminate false returns from the AUV itself. The next 
Step is the sequential evaluation of all of the remaining bins and testing the bin value 
against a threshold value. A threshold value of seven was used. If the bin strength 15 
greater than the threshold value, it is a candidate return. The next step is to compare it to 
the current maximum value. The comparison between bin values is done by weighting 
the closer bins such that a distant strong contact will not obscure a closer weaker contact 


(which may present a collision threat). 


2. Position Problems 


The main disadvantage of the tank testing is the lack of positioning data available to 
the Phoenix. Without an accurate dead reckon position for a moving vehicle, sonar data 
is useless since a bearing and range mean nothing without a point of origin. Given these 
limitations and separate problems with the dead-reckon model, the only useful sonar 
testing that was accomplished in the test tank was performed with the Phoenix stationary 


and in a known position. 
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Raw Range, Data (64 bins) 


Bin Thresholding 


Thresholding 


Strength Comparison 





Figure 5.3. Sonar Preprocessing Algorithm. 


3. Testing Results 


Tank tests conducted a 360 degree scan, and the data was processed by the sonar 
module, with the results being evaluated for correctness. These tests were performed 
with a mine-like object in the tank, to evaluate the classification rules. The plots and 
output files demonstrate the classification of a mine-like object, in the test tank using a 
stationary AUV. Sonar detection, localization and classification results were satisfactory 
as demonstrated by Fig 5.4 and Fig. 5.5. The preprocessed sonar data and the fitted line 
segments are shown in Fig. 5.4. The objects created by the module are shown in Fig. 5.5, 


with the mine-like object classified at coordinate (4.06, 0.25) with a radius of one foot. 


D. SEA WATER TESTING 


1. Moss Landing Harbor 


The first ever sea-water testing of the Phoenix took place at Moss Landing Harbor 
in January 1996. Many problems were discovered during this testing. The initial tests 
allowed for the evaluation of many systems that cannot be tested in the tank i.e., GPS and 


DiveTracker. Unfortunately the positioning data of the Phoenix was not as accurate as 
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needed to accomplish the initial transit/search/transit mission designed for the harbor. 


Poor dead reckoning and hardware reliability problems produced many unusable results. 
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Figure 5.5. Objects from Tank Testing. Note Small Mine-Like Object 


at (4.0,0.0). 
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2. Real World Situations 


The sonar data gathered from stationary and moving scans were used to test the 
algorithms in a true sea-water environment. The major improvement to the algorithm that 
was produced by this testing was the line ending condition of two consecutive zero 
returns. Prior to the harbor testing all sonar testing was done in man-made environments. 
Enclosed conditions produced the anomaly of always receiving a valid sonar return, and 


therefore the condition of no return was not discovered until Moss Landing. This was an 


excellent result. 


3. Data and Results 


The data gathered during the two weeks of testing was not as useful as originally 
expected. This was due to the lack of accurate position data while the Phoenix was 
transiting. Stationary data was gathered and the results were shown in Figs. 4.1 and 4.2. 
The output circle, world.input file is shown in Fig. 5.6 demonstrating the format of the 


input to the replanner module. Object radii equal to 1.0000 indicates that these circles 


were generated to approximate a wall. 


# OBJECT X Y Z Radius 
Circle 9.6522 13.0779 2.0000 1.0000 
Circle 10.4248 13.6633 2.0000 1.0000 


Circle 11.1974 14.2488 2.0000 1.0000 
Circle 11.9700 14.8342 2.0000 1.0000 
Circle 12.7426 15.4197 2.0000 1.0000 





Figure 5.6. Circle World Input File from Moss Landing Data. 
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E. POOL TESTING 


1. NPS Pool 


After the results of the Moss Landing testing were evaluated we decided to attempt 
further testing in the swimming pool at NPS. The goal was again a mission of detecting, 
localizing and classifying a mine-like object. The lack of accurate position information 
was once again the pitfall. 

A very successful mission was accomplished during the pool testing. The Phoenix 
was placed in a known location and a sonar search was conducted while the Phoenix was 
stationary. This experiment demonstrated the ability for all software components to be 
running together and achieve real-time performance from the sonar module. The 
pre-processed sonar data and fitted line segments from this experiment are shown in 
Fig. 5.7. The centers of the circles produced for path planning are plotted in Fig. 5.8. 
The mine-like object was detected at coordinate (30,33-36), although at this range with 
the ST725 the object only produced four returns and was classified as "unknown". The 


ability to demonstrate the real-time capabilities of this module was an excellent result. 
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Figure 5.7. Processed Sonar Data and Fitted Line Segments. 
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Figure 5.8. Circle Representation of the Sonar World. Note Small 
Mine-Like Object at (30,35). 


F. FOLLOW-ON TESTING 


1. New Virtual World 


The data that was gathered from the Moss Landing testing was implemented into 
the virtual world. The virtual world was also updated to provide sonar data based on the 
bearing of the sonar head, provided by the sonar module, and the graphical representation 
of objects in the virtual world. A computational geometric sonar model provided returns 
accurate within inches, with approximately a five percent error rate in generated returns. 
This new version of the virtual world was used to perform sonar classification and path 
replanning tests. Having a complete geometric model and complete real-time 
visualization of sonar bearings and ranges immediately clarified several difficulties, 
enabling immediate correction of several long-standing problems. This was merely one 
of many occasions where visualization improvements resulted in suprisingly profound 


insights which were previously elusive. 
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2. Results 


The results of this testing were excellent, as an end-to-end mission of detecting, 
localizing, classifying and replanning around a mine-like object was accomplished. A 
picture of the mission running is shown in Fig. 5.9. The output from the sonar module is 
shown in Fig. 5.10 displaying the sonar representation of the walls and the mine-like 
object. The raw sonar data 1s shown in Fig. 5.11. The circle world output from the 


mission is plotted in Fig. 5.12. 
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"Figure 5.10. The Sonar Module's Circle World from 
Virtual World Data. Cylinders Shown Here Represent 
Classified Objects from Fig. 5.12. 
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Figure 5.11. Raw Sonar Data with Fitted Lines from Virtual World Mission. 


NPS AUV sonar outputs 


ERAL obD26cti ve 


? 


(ЕК) 


North -» (x world) 


70 80 90 
East -» (y.world) [ft] 





Figure 5.12. Classified Objects Created from Virtual World Mission. Note the Curved 
Object Centered at Coordinate (20,46) Was Classified Mine-Like. 
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С. SUMMARY 


Testing was a four step process. The initial testing done in the virtual world 
allowed for the testing of the sonar module without the overhead of deploying the vehicle. 
This was used to test and refine the basic algorithms. The virtual world also supplied the 
ability to establish the communications between all of the parts of the software 
architecture. The next step of tank testing, was useful in the refinement of the hardware 
interface portion of the sonar module, but as mentioned above the lack of accurate 
position data limited the amount of testing possible. The next testing, accomplished in 
sea water, provided useful insights to the ability of the sonar and the algorithms. Many 
hardware failures and less-than-expected accuracy of position data limited the amount of 
useful results gathered from the sonar system during the Moss Landing tests. The fourth 
set of tests were performed using a greatly enhanced geometric sonar model in 
simulation. Accurate position data, accurate returns and some noisy returns were 
successfully analyzed and classified using vehicle hardware and vehicle software in real 
time. 

The many difficulties involved with the deployment of the Phoenix, e.g., hardware 
failures and logistic support, demonstrate what an invaluable asset a virtual world is in 
the development of software. Despite the disappointing shortfalls of end-to-end system 
testing, enough successful tests were conducted using the sonar module aboard Phoenix 
to conclude that real-time sonar classification is achievable. We believe we have a 
working system now. Further in-water testing is needed to tune coefficient choices and 


validate overall system performance in a variety of real-world situations. 
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VI. CONCLUSIONS AND RECOMMENDATIONS 


A. CONCLUSIONS 


1. Real-Time Classification 


The sonar module developed by this work has shown the ability to process sonar 
data in real time. The real time classification of objects was not accomplished in an 
untethered waterborne mission, due to many hardware and software problems, although 
subsequent testing produced real-time classification in the virtual world. The dead 
reckon position data of the Phoenix is not currently accurate or consistent enough to 
support the real-time classification of sonar objects when underway. The sonar module 
does process the sonar data received during waterborne testing and produces the required 
outputs in real-time. Without reliable position information this data gathered cannot be 
verified. Nevertheless the correctness of the algorithm has been demonstrated using 
sonar data gathered with a fixed position and a 360 degree sonar search conducted using 
vehicle hardware. With these two positive results, it is evident that real-time object 
classification is achievable. Further improvements in dead reckoning are likely and 


corresponding sonar results will be reevaluated. 


2. Collision Avoidance 


A simple collision avoidance algorithm has been implemented in the Phoenix. The 
results of this algorithm are independent of the actual position of the Phoenix. The 
algorithm uses relative sonar range and bearing for the determination of a collision threat. 
This simplicity is a desirable feature, since it will protect the vehicle regardless of the 
navigational accuracy. More testing and additional development will no doubt further 


improve collision avoidance capabilities. 
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3. Object Representation 


The representation of classified objects, for the purposes of path planning, is 
performed with circle representations as shown in Fig. 5.3. This data 1s shared with the 
replanner module by creating a file, which 1s later used by the replanner module as the 
input for a path planning process. Circle representations are adequate for most (if not all) 


target obstacles encountered by an AUV. 


B. RECOMMENDATIONS FOR FUTURE WORK 


1. Testing 


The current Tactical level needs to be further tested with adequate dead reckon 
position information. Performing a complete mission will demonstrate the capabilities 


and/or improvements needed for all current software. 


2. VxWorks 


The need for a shared memory system is evident by the large amount of message 
passing required by the current implementation. The shared-memory needs of the tactical 
level combined with the real-time requirements of the execution level can both be 
satisfied with the implementation of the VxWorks operating system. It is likely that 
performance gains are possible using shared memory. Process profiling analysis 1s 
needed first before embarking on a system reconfiguration. Regardless of whether such a 
transition is made, current results show that shared memory is not required and a standard 


Unix approach can work. 


3. Video Camera Correlation with Sonar 


The next logical step in the MCM efforts of AUVs is to use a camera to provide 
visual support of the classification performed by the sonar module. The 1dea here is that 
once a mine-like object is classified, the AUV can transit to a closer location and acquire 


visual confirmation, or provide the new classification of the object. Image processing 
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will remain independent from sonar classification, and is not needed for mine-like object 


classification or safe path planning. 


4. Expanding Classification Rules 


The current implementation has demonstrated the ability to process sonar data in 
real time and create line segments from that data. The ability to build and classify objects 
has also been accomplished, but the need for more classification rules 1s evident. Now 
that the real time problem of sonar classification has been solved, the next improvement 
is the expansion of the rules for detailed classification. We want to be able to 
discriminate between mines, rocks, fish and other moving entities. We also want to 
combine "blobbed" data points which come from the same target but do not yet provide 


adequate resolution for line fitting. 


S. Improved Collision Avoidance Reactions 


The need for improved collision avoidance actions is obvious. The current 
implementation is a fail-safe method suitable for the current testing. Improvements will 
be needed once the platform is ready for more complex testing. The improvements can 
be made both at the OOD level, by taking less severe actions in accordance with the 
phase of the mission, and at the sonar module level by creating another message (e.g. 
"collision, warning") that might occur at a farther range from fast-moving objects to 
provide the OOD more time to react to the situation. Another improvement that can 
easily be made is for the OOD module to replan after avoiding the collision, instead of 


aborting the mission which the current implementation does. 


6. Virtual World Sonar Model 


The virtual world provides sonar data for objects defined in the virtual world, by 
the user. The next step in the progression of the virtual world is the implementation of a 
realistic noise distribution for the sonar data, in order to present data that is comparable to 


data collect in waterborne experiments. The current sonar data produced by the virtual 
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world is an excellent representation of the ST1000 sonar, as it returns only a range and 
bearing. An implementation that returns a 33 byte data string similar to the ST725 (or the 
ST1000 in scanning mode) could be very useful in further testing of raw sonar 
pre-processing. This implementation would require valid sonar returns on the same 
bearing for multiple targets and would provide for the testing of pre-processing 
algorithms. This would help improve the ability to locate weaker closer contacts that can 


be masked by farther stronger contacts. 


7. ST1000 Implementation 


Due to the difficulties experienced with the waterborne testing, the ST1000 was 
never fully implemented as an available sonar to the sonar module. The code for 
communications with the ST1000 is already written. The testing required deals mostly 
with the processing of the returns in the profiling mode. Operating the ST1000 in the 
scanning mode would work identically to the ST725. The implementation of the ST1000 
will also require three-dimensional transformations. These transformations will be 
required due to the fact that the ST1000's one degree conical beam will be more sensitive 
to the AUV's pitch and roll than the 24 degree vertical beam of the ST725. This is a 


straightforward task for implementation. 


C. SUMMARY 


This work resulted in a fully implemented sonar module for the Phoenix. 
Improvements were made to the previous algorithm with the addition of checks for loss 
of sonar returns and proximity checks between returns. Further improvements were made 
in the polyhedra building with the algorithm being modified to allow for combination of 
segments that are produced in any scanning direction. Although the entire mission of 
detecting, localizing and classifying a mine-like object was not quite accomplished due to 
other problems, a major step was taken with the demonstration of the sonar module's 


ability to produce real-time in-water results detecting and localizing a mine-like object. 
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Subsequent testing in the virtual world demonstrated convincing real-time detection, 


localization and classification of a mine-like object. 
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APPENDIX A. SOURCE CODE FOR SONAR MODULE 


ПА eee een eee NAN AIK KK KK KK KK AKK KK KK KK KK EKA CK Ok Re Ke e ok he kk ke eoe 


FILENAME: 


AUTHOR: 
DATE: 


PURPOSE: 


REVISION: 


FUNCTIONS : 


эолат-с 


Mike Campbell 
15 March 1996 


Gathers all of the sonar data and performs real time 
object classification to support mine-hunting and supply 
run-time collision avoidance. 


This code is constantly being improved and expanded current 
revision is available at: 
http://www.stl.nps.navy.mil/-auv/tactical/ 


sonar() 

linear. fitting() 
start segment() 
ааа to line() 

add circle() 
normalize() 
normal2() 

normal () 

struct LINE_SEG *end_segment () 
reset accumulators() 
build polyv() 

Print list() 
classify_poly() 

Power () 

quadrant () 

triangle area() 

init next poly() 
end, polv() 


KR KKK KKK KK KKK KKK KEKE KKK KKK KKK KKK KK KR KR KK KKK KK KKK KKK KEK KKK KKK KK KKK KKK KK RK K / 


/* #include 


Pyxworks.h” + */ 
tinc lude <stdio.h> 


у ude <stdlib.h> 


#include <math.h> 
#include <ctype.h> 


#include <string.h> 
tinie lude <stddef.h> 
#include "sonar_globals.h" 


#include "../execution/globals.h" 
Pane lucdem ехесц лтлоп/аевлпег ћ" 
#include "../execution/statevector.h" 


#define RECORD SIZE sizeof (struct LINE_SEG) 
#define MAXLINE 132 


vo1id Janéar fitting ():; 
void start, segment(); 
void add. to line(); 


void add circle(t); 
double normaliz(); 
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double normal(í); 


struct LINE SEG *end segment(); 


void reset accumulators(); 
võid nuila poly), 

voard print liseli: 

void classify poly(); 
double Power(); 

int quadrant; 

double triangle area(); 
void init next poly: 
void end, poly(); 

double normal2(); 


char Sonar String. back[MAXBUFFERSIZE]; 
char Sonar data[MAXBUFFERSIZE]; 
char Sonar Read to clear[MAXBUFFERSIZE]; 


extern int Sonar to COD J Ed(2], OOD Eo Sonar Баја је 


extern int Sonar telemetry fd[2]; 
extern int  initialize sonar. systems(); 
extern double Ping. Sonar(); 


[RR RRR KKK KKK KKK KK KEK KK KEKE KKK KKK KK KEK KKK KKK KR KKK KR KKK KKK KR KKK KKK ko ko ck ck ck kk ko ko 


FUNCTION: sonar () 

AUTHOR: Mike Campbell 

DATE: 4 March 1996 

PURPOSE: Handles the communications within and outside of this 
module 

RETURNS: none, sends sonar data through socket comms to calling 
function. Ceacttcal,c) 


ЕКА К КК К КЕКЕК КЕ ИМ 


void sonar(void) 


/* Open file, testing for success */ 


lt ((outfile — fopen ("data points sonar*'; 


( 


printi ( Error opening өше леан”; 


exit (0); 


1: ((ошЕ2 = fopen ("1лпе segments sonar”, 


printf Error оре та слан file је 


exit(0); 


тї ((оБлесЁ е = topen 


printf("Error Opening QUC Бате ки); 


exit(0); 


("objects.sonar", 


м) == пне а 
NU == ( (FILE =) 0) 
“w)) == ((PILE СІРІ 


) 


ле (пем world = fopen("new world.sonar", "w")) zs*((FILE *)*0)) 
{ 
printf("Error opening new world file. Mn"); 
) 
INE IwoPiHEDPGS-Cftopen( world'; "r")) == ((FILE *) 0)) 
{ 
printf("World file does not exist. Wn"); 
) 
else 
| 
while (fgets(line, MAXLINE, worldfile) ) 
{ 
sscanf(line,"$f $f Zf Zf ФЕ 9Е ЗЕ ФЕ SE 91 91 91", 
&С1,&С2,&С3,&С4,&С5,&С6,&С7,&С8,&С9, 
Sil 8$12 513); 
world[poly num].start = tl; 
world[poly. num].end = t2; 
world[poly num].head of poly = NULL; 
world[poly num].headx =з 
world[poly_num] .heady = t4; 
world[poly num].tailx ЕЕ: 
world[poly num].taily uto 
world[poly_num] .centroidx =. 
world[poly. num].centroidy zi 
world[poly num].area - t9; 
world[poly num].seg count = 
мог1а [ро1у num].status = 13; 
worldipoly num]. classification= 13; 
++poly_num; 
} 
) 
segment data.theta - 0; 
segment data.r - 0; 
segment data,.num points = 0; 
segment data.line status = 0; 
while (1) 
( 
if (read(Sonar telemetry fd[0],Sonar. data, MAXBUFFERSIZE)-- -1) 
ІІ 
else 
( 
parse telemetry string(Sonar data); 
if (read(OOD to. Sonar. fd[0], Sonar. data, MAXBUFFERSIZE)--2 -1)() 
else 
( 
СЕС Sonar data, INITIALIZE") == 0) 
{ 
if (LOCATIONLAB) 


{ 


sprintf(Sonar String. back," SONAR INITIALIZED"); 


Sonar. mode - 


0; 
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SONAR_ Е =. 
Scan direction - -1; 
write(Sonar to OOD fd[1],Sonar, String back,MAXBUFFERSIZE); 


else 
{ 
St path - initialize sonar. systems(); 
Jf {st pach > 0) 
{ 
sprintf(Sonar String back,"SONAR INITIALIZED"); 
Sonar mode - 0; 
SONAR 725 bearing = 0; 
Scan, direction - -1; 
write(Sonar to OOD fd[1],Sonar String back,MAXBUFFERSIZE); 
printf("£sWMn",Sonar, String. back); 
) 
else 
{ 
sprintf(Sonar, String back,"SONAR, INITIALIZED FAILED"); 
write(Sonar to OOD fd[1],Sonar. String back,MAXBUFFERSIZE); 


) 
} 


else if (strcmp (Sonar_data, "SONAR_SEARCH") == 0) 
Sonar_mode = 1; 
| 

else if (strcmp (Sonar даска, "ВОТАТЕ ФЕАВСН") == 0) 
{ 
Sonar_mode = 2; 


} 

else if (strcmp (Sonar data, "QUIT") 
( 
classify. poly (poly. num); 
Print tase): 
fclose(textfile) ; 
fclose(outfile); 
fclose(out2); 
fclose(worldfile); 
fclose(new world); 
exit(0); 


|| 
Н 
© 


) 
Switch (Sonar. mode) 
{ 
сазе -1: 
ркеак; 
case 0: 
if ((Scan direction -- -1) && (SONAR 725 bearing »- 60.0) ќе 


(SONAR 725 bearing «- 300.0)) 
Scan, direction - 1; 
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else if ((Scan direction -- 1) && (SONAR 725, bearing «- 300.0) && 
(SONAR, 725. bearing »- 60.0)) 
Scan direction - -1; 
if (SONAR, 725. bearing > 170 && 5ОМАК 725 bearing « 190) 
Hin chreshold = T2; 
else 
bin_threshold = 4; 
if (LOCATIONLAB) 
{ 
SONAR 725 range - AUV ST725 range; 
) 
else 
SONAR, 725 range - 
Ping Sonar(st, path,Scan direction,bin threshold); 
SONAR 725 bearing = 
normal2(SONAR 725 bearing -* step size *-0.9*Scan direction); 
break; 
case 1: 
if (Search status == 0) 
( 
if (!LOCATIONLAB) 
center sonar(st path); 
x search = х; 
y Search - y; 
SONAR, 725 bearing = 0.0; 
Search status - 1; 
) 
if (SONAR, 725. bearing » 170 && SONAR 725 bearing « 190) 
bin, threshold =12; 
else 
bin threshold = 6; 
if (LOCATIONLAB) 
SONAR_725_range = AUV_ST725_range; 
else 
SONAR_725_range = Ping_Sonar(st_path,-1,bin_thresholdq) ; 
SONAR 725 bearing += step size *0.9; 
if (SONAR 725. bearing » 360) 
( 
SONAR 725. bearing - normal2(SONAR, 725 bearing); 
Search status - 0; 
Sonar mode = 0; 
sprintf(Sonar String back,"SONAR, SEARCH COMPLETE"); 
write(Sonar to OOD fd[1],Sonar String back,MAXBUFFERSIZE); 
) 
ркеак; 
сазе 2: 
if (Rotate status == 0) 
( 
lf (!LOCATIONLAB) 
center sonar(st path); 
SONAR 725 bearing - 0.0; 
Rotate status - 1; 
Rotatewecount = 0; 


) 
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if (LOCATIONLAB) 
SONAR 725 range - AUV ST725 range; 
else 
SONAR 725 range - Ping. Sonar(st path,0,8); 
Rotate count += 0.5; 
if (Rotate count > 200) 
( 
Rotate status - 0; 
Sonar mode - 0; 
sprintf(Sonar String back,"ROTATE SEARCH COMPLETE"); 
write(Sonar. to OOD fd[1],Sonar. String. back,MAXBUFFERSIZE); 
) 
break; 
) 
if (SONAR, 725 range »- 0.1) 
{ 


one_bad_range = 0; 

ТЕ (Sonar_mode != 1 && t > 1.0 && !LOCATIONLAB) 
{ 

x_return = x + 3*cos(normal(psi*M_PI/180)) + 


cos (normal (psi*M_PI/180+ 


SONAR, 725. bearing*M PI/180))*SONAR 725, range; 


y return - y + 3*sin(normal(psi*M PI/180)) - 
Sin(normal (psi*M_PI/180+ 


SONAR, 725 bearing * M PI/180))*SONAR 725 range; 


) 

else if (Sonar. mode == 1 && t > 1.0 && !LOCATIONLAB) 

{ 

x return - x search « 3*cos(normal(psi*M PI/180)) + 
cos (normal (psi*M_PI/180+ 


SONAR 725. bearing*M PI/180))*SONAR 725 range; 


y return - y search « 3*sin(normal(psi*M PI/180)) + 
Sin(normal (psi*M_PI/180+ 


SONAR 725 bearing*M PI/180))*SONAR, 725 range; 


) 
else if (t > 1.0) 
{ 
x_return = x + 3*cos(normal(psi*M PI/180)) + 
cos(normal(psi*M PI/180-« 


AUV ST725 bearing*M PI/180))*SONAR 725 range; 


y return = у + 3*sin(normal(psi*M_PI/180)) + 
Sin(normal (psi*M_PI/180+ 


AUV ST725 bearing*M PI/180))*SONAR. 725. range; 


) 


lf (t >= 1:0 && (((x-return = ота х) “(x return = опије 


(у return - old y)*(y return - olid-y)) >= 9 ЕВ 


sprintf(Sonar. String back,"SONAR, 725 $1f $1f 1f", 


SONAR 725 bearing ,SONAR 725 range,SONAR 725 strength); 


write(Sonar to OOD fd[1],Sonar. String back,MAXBUFFERSIZE); 
fprintf(outfile,"t]I£f SLE ІР Nu',xcretuEDng сви 

,AUV ST725 bearing); 
fflush (outfile) ; 
old x = x return; 
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old_y = y_return; 
linear fitting(); 


else 
{ 
sprintf(Sonar. String back,"SONAR 725 $1f %1Е %1Е", 
SONAR 725 bearing, SONAR 725 range, SONAR 725 strength); 
write(Sonar to OOD fd[1],Sonar String back,MAXBUFFERSIZE); 
) 
SONAR 725 range - 0.0; 
) 
else if (one bad return -- 1 && segment data.line status » 1) 
( 
build poly(end segment,poly num); 
reset accumulators(); 
) 
else 
one bad return - 1; 
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FUNCTION: Power() 

AUTHOR: Mike Campbell 

DATE: 4 March 1996 

PURPOSE: Provides a function that raises numbers to powers. 
RETURNS: A double of Base raised to the Exp. 
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double Power(Base,Exp) 


double Base; 
Ime EXD; 


NA ALOON = ED; 
double Total = 1.0; 
double BaseNum = Base; 


while (Loop) 
{ 
1£(Loop > 0) { 
Total Total * BaseNum; 
LOOD =; 
} 
else 


{ 
Total = Total / BaseNum; 


| 
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Гоор++; 
} 
} 


return Total; 


[^к кк КККК КАК К Кккк АВА АЛА А ОВА X аа ка ка ПОЉУ А А КАН 


РОМСТТОМ: quadrant() 


AUTHOR: Mike Campbell 
DATE: 4 March 1996 
PURPOSE: Returns the quadrant of the angle as a number from 0 to 3, 


with 0 representing the +Y and -X quadrant and rotating CW 
from there. 


RETURNS: An integer between 0 and 3. 


ec cec ce e e ee ke ee ce e eoe e e ce ehe he ee ke e e e e ke e ke e e e e ce se ke ehe he ke e eek e e e e e ke ke ke e he ke e ke ke e ke he ke ke kk kk x / 


int quadrant(Angle) 


double Angle; 


if(Angle » M PI/2) 
returnaos 

else if(Angle » 0) 
return 4: 

else if(Angle » -M PI/2) 
return 2; 

else 
return 3; 


f * Noe ОКО О ОКО О О О О ОКО КА ЈЕО ЈКОК e 


FUNCTION: triangle area() 

AUTHOR: Mike Campbell 

DATE: 4 March 1996 

PURPOSE: Calculates the area of a triangle represented by the points 


Xl;Yl Ха и апсы v. 


RETURNS: A double representing the area of the triangle P1P2P3. 
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double triangle_area(X1,Y1,X2,Y2,X3,Y3) 


doubbe ey te 2 xcu S 
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double Ans; 


КЫШ о (Ze ee УЗ = у1))—((Х3 = X1)*(Y2-Y1)))); 
return Ans; 
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FUNCTION: normalize() 


AUTHOR: Mike Campbell 
DATE: 4 March 1996 
PURPOSE: Accepts a double representation of an angle (in radians) 


and returns an angle between -PI and +PI. 


RETURNS: A double between -PI and +PI. 
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double normalize(theta) 


double theta; 


( 
double Ans; 
if (theta « -M PI) 
( 
Ans = theta -« 2*M PI;) 
else if (theta »- M PI) 
( 
Ans - theta - 2*M РТ;): 
else 
Ans - theta; 
return Ans; 
) 
[KAKAK ke ke ke e e kc ee e e e e e ke e ke ke e ke ke ce ck ke ke ck ce ke ke ck ke ke ce ke Sce e ke ke e ke ee ke ke e e e ck kc e ck sk ke ke ke kk kc ko ko 
EUNCTTON: normal () 
AUTHOR: Mike Campbell 
DATE: 4 March 1996 
PURPOSE: Accepts a double representation of an angle (in radians) 


and returns an angle between 0 апа 2PI. 


RETURNS: A double between 0 and 2PI. 
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double normal(theta) 


double theta; 
( 
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double Ans; 


if (theta 


О) 


Ans = theta Ft 22NWPRC 
else if (theta >= 2*M PI) 
Ans - theta - 2*M PI; 


else 


Ans - theta; 
return Ans; 


Джил а ВАРВАРА UI 


FUNCTION: 


AUTHOR: 


DATE: 


PURPOSE: 


RETURNS: 


поста (7) 

Mike Campbell 

4 March 1996 

Normalizes numbers (in degrees) between 0 and 360. 


A double between O0 and 360 degrees. 
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double normal2(theta) 


double theta; 


( 


double Ans; 


if (theta « 0.0) 
Ans - theta + 360.0; 
else if (theta »- 360.0) 
Ans = theta - 360.0; 


else 


Ans = theta; 
return Ans; 
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BUNC TION: 


AUTHOR: 


DATE: 


PURPOSE: 


linear fitting) 
Mike Campbell 
4 March 1996 


This procedure controls the fitting of range data to straight 
line segments. First it collects four data points and establishes 
a line segment with it's interim data values. After the segment 
is established, the procedure tests each subsequent data point 

to determine if it falls within acceptable bounds before calling 
the least squares routine to include the data point in the line 
segment. After inclusion of the data point the segment is again 
tested to ensure the entire set of data points are linear enough. 
If any of the tests fail, the line segment is ended and a new one 
started. The completed line segment is stored in a data structure 
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called segment, and segments are linked together in a linked list. 


RETURNS: none, sends sonar data through socket comms to calling 


fonction (васе сас) 
ELLEN S USUNKNUSKOEOROR OK e n oK ok eoe ok ok koe e de eoe ke ke ok oe ok e à k e  J 


void linear. fitting() 


int num points, line status; 
double theta, r, sigma, delta,del y, del x; 
struct LINE SEG *finished segment; 


theta - segment data.theta; 

Ý = segment_data.r; 

num_points = segment_data.num_points; 
line_status = segment_data.line_status; 
del x - x return - segment data.endx; 

del y - y return - segment data.endy; 


Ја“ FIRST CHECK TO SEE IF NEW POINT IS TO FAR FROM LAST POINT TO INCLUDE */ 


1f(num polints > 0) 
| if (del_x*del_x + del_y*del_y > 4.0) 
E. (line status » 1) 
| finished segment = end_segment (); 
build poly(finished segment,poly. num); 
} 


reset accumulators(); 


segment data.num points = 0; 
segment data.line status = 0; 
line status = 0; 
nüm points =- 0; 
} 
} 
line status ss 0) /* not enough data points yet */ 


{ 
segment_data.initx[num_points] 
segment_data.inity{num_points] 
segment_data.endx = x_return; 
segment_data.endy = y_return; 
segment_data.num_points += 1; 
it (num points == 3) 

{ 

start segment(); 

segment data.line status - 1; 


) 


x return; 
y return; 


) 


else 


( 
Sgmde Sscgmeutedata-sgm»deltassq / (double) num points; 
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delta 2 x return * cos(thetad) + y return ^К = 11: (Не - = е: 
if (fabs(delta) « residual, tolerance)|| 
fabs(delta) « (sigma * sigma weighting) 
( 
switch (line status) 
( 
case 1: 
segment data.num points += 1; 
add to line(x return, y return); 


if (segment data.line status == 1) 
segment data.line status = 2; 
else if (segment data.line status == 5) 


{ 


reset accumulators(); 


Segment data.initx[0] = segment data.initx[1]; 
segment data.inity[0] = segment data.inity[1]; 
segment data.initx[1] = segment data.initx[2]; 
segment data.inity[1] = segment data.inity[2]; 
segment data.initx[2] = segment data.initx[3]; 
segment data.inity[2] -» segment data.inity[3]; 
segment data.initx[3] гз x return; 
Segment data.inity[3] = y return; 
segment data.num points - 4; 
start segment(); 
segment data.line status - 1; 
} 
break; 


case 2: 
segment data.num points += 1; 
add to line(x return, y return); 
if (segment data.line status -- 5) 
( 
finished segment - end segment(); 
build poly(finished segment,poly. num); 
reset accumulators(); 
segment data.initx[0] = x return; 
segment data.inity[0] -» y return; 
segment data.num points - 1; 
segment data.line status - 0; 
segment data.endx-x return; 
Segment data.endy-y return; 
) 
break; 
case 3: 
segment data.initx[1] » x return; 
segment data.inity[1] -» y return; 
segment data.line status = 4; 
break; 
case 4: 
segment data.num points += 1; 
add to line(segment data.initx[1], segment data.inity[1]); 
if (segment data.line status == 5) 
( 
finished segment - end segment(); 
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build poly(finished segment,poly. num); 
reset accumulators(); 
segment data.initx[2] x return; 
segment data.inity[2] » y return; 
segment data.num points = 3; 
segment data.line status - 0; 
segment data.endx-x return; 
segment data.endy-y return; 
) 
else 
( 
segment data.num points += 1; 
add to line(x return, y return); 
if (segment data.line status -- 
( 
finished segment - end segment(); 
build poly(finished, segment,poly num); 
reset accumulators(); 
segment data.initx[0] x return; 
segment data.inity[0] y return; 
segment data.num points - 1; 
segment data.endx-x return; 
segment data.endy-y return; 
segment data.line status - 0; 
) 
else 
segment data.line status - 2; 


2) 


) 


break; 


) 


else 
( 
switch (line. status) 
{ 
сазе 1: 
сазе 2: 
segment data.initx[0] x_return; 
segment_data.inity[0] y_return; 
segment_data.line_status = 3; 
break; 
case 3: 
finished segment - end segment(); 
build poly(finished segment,poly num); 
reset accumulators(); 


segment data.initx[1] » x return; 
segment data.inity[1] -» y return; 
segment data.num points - 2; 


Segment data.endx-x return; 
segment data.endy-y return; 
segment data.line status - 0; 
break; 
case 4: 

finished segment - end segment(); 
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build poly(finished segment,poly num); 
reset accumulators(); 


segment data.initx[2] -» x return; 
segment data.inity[2] - y return; 
segment data.num points - 3; 


segment data.endx-x, return; 
segment data.endy-y return; 
segment data.line status 
break; 


ur 


ДЖЕКА Ао 


FUNCTION: Start segment() 

AUTHOR: Mike Campbell 

DATE: 4 March 1996 

PURPOSE: This procedure establishes a new line segment with the four 
data points contained in segment data.init(x and y). It 
writes the appropriate data to the interim values in 
segment data. 

RETURNS: 
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void start segment() 


double theta, r, mux, muy, muxx, muyy, muxy,sds = 0; 
ЛЕ S 
segment data.start time - t; 
segment data.startx - segment, data.initx[0]; 
segment data.starty - segment data.inity[0]; 
segment data.endx - segment data.initx[23]; 
segment data.endy = Segment data.inity[3]; 
for (i - ОБЕ 572227551) 
( 
segment data.sgmx += segment data.initx[il; 
segment data.sgmy += segment data.inity[i]; 
segment data.sgmx2 += Power(segment data.initx[1i],2); 
segment data.sgmy2 += Power(segment_data.inity[i],2); 
segment data.sgmxy += segment data.initx[i] * 


segment data.inity[i]; 


mux .— Segment data.sgmx/4.0; 
muy = segment data.sgmy/4.0; 
пихх - segment data.sgmx2 - Power(segment data.sgmx,2)/4.0; 
тцуу = segment data.sgmy2 - Power(segment data.sgmy,2)/4.0; 
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muxy - segment data.sgmxy - (segment data.sgmx 
* segment data.sgmy) /4.0; 


a£ (-2.0 *emuxy != 0 || muyy - muxx != 0) 
theta = (atan2( -2.0 * muxy, (muyy - muxx))) / 2.0; 
r = mux * cos(theta) + muy * sin(theta); 


ени = 0; 2 < 4; +435) 
{ 


sds += Power(segment data.initx[j] - mux,2) * Power(cos(theta),2); 
sds += Power(segment data.inity[j] - muy,2) * Power(sin(theta),2); 
sds += 2.0 * (segment data.initx[j] - mux) * 

(segment data.inity[j] - muy)* cos(theta) * sin(theta); 


) 
segment data.sgm delta sq - sds; 
segment data.theta - theta; 
segment data.r - r; 
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РИМСТТОМ: add. to line() 
AUTHOR: Mike Campbell 
DATE: 4 March 1996 
PURPOSE: This procedure checks to see if the current return fit the 


current line segment based on range and thinness ratio. 


RETURNS: None. sets line status -- 5 if cannot add return to current 
segment. 
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void add to line(x,y) 


double x,y; 


double num points; 

double m major, m minor, d major, d minor, theta, r; 
double mux, muy, muxx, muyy, muxy, sds; 

mm 1: 


num points - (double)segment. data.num points; 
segment data.sgmx += x; 

segment data.sgmy += у; 

segment data.sgmx2 += Power(x,2); 

segment data.sgmy2 += Power(y,2); 

segment data.sgmxy += x * y; 


тих = segment data.sgmx / num points; 
muy = segment data.sgmy / num points; 
тихх = segment data.sgmx2 - Power(segment data.sgmx,2) / num points; 


muyy = segment_data.sgmy2 - Power(segment data.sgmy,2) / num points; 
muxy - segment data.sgmxy - 
(segment data.sgmx*segment data.sgmy) / num points; 
m major - (muxx+muyy) /2.0 -sqrt ((muyy-muxx) * (muyy-muxx) /4.0 + 
Power (muxy,2)); 
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m minor = (muxx+muyy)/2.0 +sqrt( (muyy-muxx) * (muyy-muxx) /4.0 + 
Power (muxy,2)); 

d major 2 4.0 * Sqrt (tabs (m_minor/nume points) ). 
d minor = 420 * Sart (tabs (m majer/mumepoinmtes)). 
if ((d minor / d major) « thinness requirement) 

( 

if (-2.0 * muxy !s 0 || muyy - muxx != 0) 

theta - (atan2( -2.0 * muxy, (muyy - muxx))) / 2.0; 

r = mux * cos(theta) + muy * sin(theta); 

sds += Power(x - mux,2) * Power(cos(theta),2); 

sds += Power(y - muy,2) * Power(sin(theta),2); 

sds += 2.0 * (х = mux) * (у = muy) * cos(theta) * Sim (theta): 

segment_data.sgm_delta_sq += sds; 

segment_data.theta = theta; 

segment_data.r = к; 

segment_data.endx = x; 

segment_data.endy = y; 

segment_data.d_major = d_major; 

segment_data.d_minor = d_minor; 

if((normal2(SONAR 725, bearing) » 336 || 

normal2(SONAR 725 bearing) « 024) && 
SONAR 725 range « 5.0) 
( 
sprintf (Sonar_String_back, "COLLISION_THREAT'" ) ; 
write(Sonar to OOD fd[1],Sonar String back,MAXBUFFERSIZE); 
printf("$sWMn",Sonar String back); 
} 
) 


else segment data.line status = 5; 
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FUNCTION: LINE SEG *end segment() 

AUTHOR: Mike Campbell 

DATE: 4 March 1996 

PURPOSE: This procedure finishes off a line segment placing it into 


the LINE SEG data structure, including the calculation of 
the orientation of the line segment. 


RETURNS: Current LINE SEG. 
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Struct LINE SEG *end segment() 


struct LINE_SEG *seq_ ptr; 
double startx, starty, endx, endy, delta, theta, r, length, t = 0; 
double bearing end,bearing start; 


Startx - segment data.startx; 
starty segment data.starty; 


И 
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епах - segment data.endx; 


endy = segment_data.endy; 

theta = segment_data.theta; 

E - Segment data.r; 

delta = startx * cos(theta) + starty * sin(theta) - г; 
startx = startx - (delta * cos(theta) ); 

starty = starty - (delta * sin(theta)); 

delta = endx * cos(theta) + endy * sin(theta) - r; 
епах - endx - (delta * cos(theta)); 

endy - endy - (delta * sin(theta)); 

length = sqrt(Power(startx - endx,2) + Power(starty - endy,2)); 
БӘСЕРЕС - (сстпсс LINE SEG *) malloc (RECORD SIZE); 
seg_ptr->headx = startx; 

seg _ptr->heady = starty; 

seg_ptr->tailx = endx; 

seg_ptr->taily = endy; 

seg_ptr->alpha = theta; 


seg_ptr->start_time = segment_data.start_time; 

seg_ptr->finish_time = t; 

if( (endx-startx != 0 || endy-starty != 0) &&(endy != 0 || endx != 0) && 
(Scar они (Starty != 0) ) 


bearing_end = atan2(endy,endx) ; 
bearing start = atan2(starty,startx) ; 


if (( (abs (quadrant (bearing_end) - quadrant (bearing_start)) != 3) && 
(bearing_end - bearing_start >= 0.0 )) || 
( (abs (quadrant (bearing_end) - quadrant (bearing_start)) == 3) && 
(bearing end - bearing start « 0.0 ))) 
seg ptr-»orientation = atan2((endy-starty), (endx-startx) ); 
else 
seg_ptr->orientation = atan2((starty-endy), (startx-endx) ); 
} 
seg _ptr->r = r; 


seg_ptr->length 
seg_ptr->dmajor segment_data.d_major; 
seg_ptr->dminor segment_data.d_minor; 
seg_ptr->next = NULL; 
if((normal2(SONAR 725 bearing) > 336 || 
normal2(SONAR 725 bearing) « 024) && 
SONAR 725 range « 5.0) 


length; 


| 


{ 

sprintf(Sonar String back," COLLISION. THREAT"); 

write(Sonar. to OOD fd[1],Sonar. String back,MAXBUFFERSIZE); 

Ратево окт „она“ String back) ; 

) 
fprintf(out2,"t LINE SEGMENT*Mn$41f %41Е %41Е %41Е\п%41Е %41Е %41Е 

%41Е\п\п", seg ptr-»start time, seg ptr-»headx, 
seg ptr-»heady, seg ptr-»orientation, 
seg ptr-»finish time, seg ptr-»tailx, seg ptr--^taily, 
seg ptr-»orientation); 

Бе (оне 2); 
return seg ptr; 


73 


УЖЕ косы В+ - s 


FUNCTION: 


AUTHOR: 
DATE: 


PURPOSE: 


RETURNS: 


reset accumulators() 


Mike Campbell 5 
4 Максћ 1996 


This procedure resets all of the cumulative segment data, 
preparing for a new segment to begin. 
None. 


хх» ук ккк ККК ЕКЕ ЖЕ ж ката ааа ааа тата аты 


void reset accumulators() 


( 
segment data 
segment data 


segment data. 
segment data. 
segment data. 


segment data 
) 


-nüm рот прееска; 


.Sgmx = 0.0; 
somy = ООо; 
sgmx2 - 0.0; 
sgmy2 - 0.0; 
.Sgmxy - 0.0; 


[E ok koc e e e e ee ke e se e ee e hee e he ee e he e oe ce e ee e e e he ese se ce e ce ok ee kc he ke ee e e ok ee ko sk koc ck ck ck ck ck ck ko 


FUNCTION: 


AUTHOR: 


DATE: 


PURPOSE: 


RETURNS: 


add. circle() 
Mike Campbell 
4 March 1996 


This procedure produces the circle representation of the 
classified objects. 


None. 


e ce cce ee ee ee che ee ce e ck e e e e See e ee ce cse e e ee se ee oe he he oe ce e ecce ce e che ce e e e ce ck sk sk e e ke kc kc e à ko kc ko ko kc ko ko ko ko kJ 


void add circle() 


char 
FILE 
double 
char 
double 
int 


headx 
heady 
tailx 
taily 


11 


newfile[22],oldfile[22]; 
*new circ Ех; 
xtemp,ytemp,rtemp,radius; 
command [MAXLINE]; 


length, headx, heady, tailx,taily,area,del_x,del_y,num_circles; 
a 


world[poly. num].headx; 
world[poly num].heady; 
world[poly. num].tailx; 
world[poly num].taily; 


area - world[poly. num].area; 
length = sqrt(fabs(((headx - tailx)*(headx - tailx) + 


(heady - taily) * (heady - taily)))); 
sprintf(newfile, "circle world.input$1i", poly num); 
sprintf(oldfile, "circle world: 1nput21t", poly num = 1); 
sprintf(command,"cp $s $s", oldfile, newfile); 


if (poly num » 0) system(command); 
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ПАН теле ес = гореп (гем 1]је,"а")) == ((FILE *) O)) 


( 


printi Errom opening new world Win"); 


} 


else 
( 
switch (world[poly num].classification) 
( 
case 1: 
radius - global. radius; 
num circles - ceil(length/global. radius); 
del x - (tailx - headx) / num circles; 
del y = (taily - heady) / num circles; 
iore 19-090. X < paum circles; ++i) 


ПЕРО ЕЕ Ете спие ptr, Circle %6.4Е %6.4Ғ %6.4Е %6.4Е\п", 
(ћеадх + i*del x),(heady + 1*del_y),z,radius); 
) 
ркеак; 
сазе 2: 
if ((sqrt (fabs (area*2/M_PI))) > 0.5) 
radius = sqrt(fabs (area*2/M_PI)); 
else 
radius = 0.5; 
Mret (new Circ ptr," "Circle %6.4f %б.4Ё %6.4Ё %6.4f\n", 
(world[poly num].centroidx / (world[poly num].seg count * 2)), 
(world[poly num].centroidy / (world[poly num].seg count * 2)), 
гасе): 
ркеак; 
case 3: 
if ((sqrt(fabs(area*2/M PI))) > 0.5) 
radius -» sqrt(fabs(area*2/M PI)); 
else 
radius = 0.5; 
Aner new Cire pir, Circle 56.41 %6.4f %6.4f 36.4f\n", 
(world[poly num].centroidx / (world[poly num].seg count * 2)), 
(world[poly. num].centroidy / (world[poly num].seg count * 2)), 
z,radius); 
break; 
) 
} 
fclose(new circ ptr); 
sprintf(Sonar String back,"REPLAN %s",newfile) ; 
write(Sonar to OOD fd[1],Sonar String. back,MAXBUFFERSIZE); 
printf("$sMn",Sonar String back); 


f kk ok ke e à ke e ke e e kk ke ke e e ke ce Sk Se e ke kc ck ck ke ke ck ce ke ce ck ce e he e he ke he e e e e e ck e e ke ke ke ke e e ke ek ek К К ЖК Ж 


FUNCTION: check if new() 
AUTHOR: Mike Campbell 


DATE: 4 March 1996 
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PURPOSE: This procedure determines if the current object just 
finished is a new object or correlates to an already 
existing object. 


var 


RETURNS: "1" if it is a new object or "O" if it is not new. 
Wok ok OR ORO ХХ ЖЕЖ КАК КЖ КЖ КЖ кк a a a a a ата” 


int check if new (new. poly) 
struct Polyhedron new poly; 
псе Е, 
double та БЈБ: 


for (1i 0: i < poly num 11+) 


{ 


IH 


ns 
S 


new_poly.seg_count * 2; 
world[i].seg_count * 2; 
if ((fabs(world[il.alpha - new poly.alpha)) « 0.2 && 
world[il].classification == I 
&& . (fabs(new poly.centroidx/ns - world[i].centroidx/s) « 1.0) && 
(fabs (new poly.centroidy/ns - world[il.centroidy/s) « 1.0) 
&& new poly.classification -- 1) 


( 
return 0; 
} 
else if ((fabs(new_poly.centroidx/ns - world[i].centroidx/s) < 1.0) && 
(fabs (new poly.centroidy/ns - world[i].centroidy/s) « 1.0) && 
(fabs(new poly.area - world[il.area) « 5.0)) 
( 


return 0; 


return 1; 


[RRR RRR RR RR e e e e e ce e e hehe ce KR RR KR KR RR KK RRR KR KR KR KKK RR RK KR RRR RR KR KR e ke e e ck ke e Sk e ke e ck kk o 


FUNCTION: Classify. poly() 

AUTHOR: Mike Campbell 

DATE: | 4 March 1996 

PURPOSE: This procedure classifies the objects based on size length 


and other characteristics. 


RETURNS: None. 


PREECE EIR KKK RRR RRA AK RK RRR RR oe voc ook oo oro eo ook oe p ОУ ОО 


void classify poly (n) 


int n; 
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double length,headx,heady,tailx,taily,area,alpha,del x,del y,num circles; 
nnt 1; 


headx world[n].headx; 
heady = worlid[n] .heady; 
tailx world[n].tailx; 
taily world[n].taily; 
area - fabs(world[n].area); 
alpha = world[n].alpha; 


length = sqrt((headx - tailx)* (headx - tailx) + 
(heady - taily) * (heady - taily)); 
if ((length > 5.0) && (area/length/length < 0.1)) 
wonldad(in)].classification = 1; 
else if ((area >= 10.0) && (area <= 100.0)) 
world[n].classification = 2; 


else 
world[n].classification = 3; 
if (check if new(world[n]) && 
(length > 0.5 || world[nl.seg count >= 2 )) 
( 
add, circle(); 
) 
else 
| 
reset accumulators(); 
poly num -- 1; 
) 


[KAKAK KK KAKAK AKK KK AKA KAKAK KK KK KK AKK KAKA KAKAK AKK KK KAKA KAKAK KAKAK KAKA KAKAK 


FUNCTION: end, poly() 

AUTHOR: Mike Campbell 

DATE: 4 March 1996 

PURPOSE: This procedure ends the polyheron and classifies it. 
RETURNS: None. 


mt cC EX EX XE UK Kk X ЖКЖ ЖКЖ ook ok ok ko kk kc ko k ok ok k kk kJ 


void end, poly(ptr,n) 


ine ns 
struct LINE SEG *ptr:; 


{ 


мета] став = T; 


TT 


classify_poly(n); 


ЈОХАНУ НИЖИХ И А ХИ НИ ИЦ ee eee 


FUNCTION: inl tonext poly () 

AUTHOR: Mike Campbell 

DATE: 4 March 1996 

РОВРОЗЕ: This procedure initializes the next polyhedron number once 


the current polyhedron is completed. 


RETURNS: None. 


ЖКХ КК KA KOK KK AK KK KOKOK Oe KO Seo SK Sk KOKOK KOK KIKIR KO KADA KALANA KAKAK KAR Aa AAK A 


void зите next. poly (per Ш} 


ІЛЕСТІ: 
struct ГЕМЕУСВЗ “ЕЕЕ; 


double del x,del y; 


world[n].head of poly s (struct LINE-:SEG. *) malloc (RECORD SIZE), 
world[n].head of poly-»next - ptr; 

ptr->prev world[n].head of poly; 

ptr-»next - NULL; 
world(n].centroidx 
world[n].centroidy 
world[n].seg count 
world[n].area = 0.0; 

world[n].alpha ptr-»alpha; 
world[n].tailx ptr->tailx; 
world[n].taily ptr->taily; 
world[n].headx - ptr-»headx; 
world[n].heady = ptr->heady; 
world[n].statuss-«07 

del x world[n].headx - world[n].tailx; 
del y - world[n].heady - world[n].taily; 
if (del x*del x + del y*del y »- 16.0) 


(ptr->headx + ptr->tailx) ; 
(ptr->heady + ptr->taily); 
i 


world[n] .classification = 1; /*long enough to be a wall*/ 
else 
world[n].classafQ3catiopD 5 3; his means unknown’ 7 


f Ke ke ce e ce ke ehe ehe e e e e e e e e e ee e e e e ke e e ce e e ee e ce se e e e ce ke e ke e e ke e ce he ke ce ce ke e ke e e ke ce sk ke ke ke kk kk ek 


FUNCTION: build ро1у() 


AUTHOR: Mike Campbell 
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DATE: 4 March 1996 
PURPOSE: This procedure builds polyhedron out of line segments. 


RETURNS: Nones 


ххх ok ok e ke ke he kk ke К+ + / 


Е: burld poly(ptr,n) 


IE n; 
Semuct LINE SEG “рст; 


struct LINE_SEG *templ = NULL; 
double tril,tri2,alphal,alpha2,bearingl, bearing2; 


if (world[n].head of poly -- NULL) 
( 
Pot next poly (ptr;n) > 
} 


else 
{ 
templ = world[n] .head_of_poly->next; 
while (templ-»next !- NULL) 


templ - templ-»next; 
if (((ptr->heady - templ->taily) * (ptr->heady - templ->taily) + 
(ptr->headx - templ->tailx) * (ptr->headx - templ->tailx)) > 36.0) 


{ 
пошт == роту тот) 
{ 
eridapoly (pen, a); 
poly_num += 1; 
init _next_pedsy (ptr,poly пит); 
} 
} 
else 
{ 
while (templ-»next !- NULL) 


templ - templ-»next; /* get to end of list */ 
ІП (П(ӘСЕС->К > 0:0) 
alpha2 = ptr->alpha; 
else if (templ->r > 0.0) 
alpha2 = normal(M PI + ptr->alpha) ; 
else 
alpha2 = ptr->alpha; 
if (templ-»r > 0.0) 
alphal - templ-»alpha; 
else if (ptr-»r » 0.0) 
alphal = normal(M PI % templ-»alpha); 
else 
alphal templ-»alpha; 
bearingl = atan2(templ->taily - y, templ->tailx - x); 
bearing2 = atan2(ptr->taily - y, ptr->tailx - x); 
if ((fabs(normal(alpha2 - alphal))) « 0.1745) 
( 
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ptr->prev = templ; 
templ-»next - ptr; 
ptr->next = NULL; 
world[n] .centroidx += (ptr->headx + ptr->tailx); 
world[n].centroidy += (ptr->heady + ptr->taily); 
world[n].seg count += 1; 
if (world[in].classification == 1) 
world[n].alpha - ((world[nl.seg count - 1)*world[n].alpha - 
ptr-»alpha)/world[n].seg count; 


tril - triangle area(world[n].headx,world[n].heady, 
world[n]l.tarilx,world[n]-.tarXly. 
ptr->headx, ptr->heady) ; 

tri2 - triangle area(world[n].headx,world[n].heady, 
ptr-»headx,ptr-»heady,ptr-»tailx,ptr-»taily); 

world[n].area += (Бе: + Беса); 


worldin]  tailx = ptr >taãailx; 
worldin]. taily =- рес бату, 


} 
else if((abs (quadrant (bearing1) - quadrant (bearing2)) != 3) && 
((bearingl - bearing2 > 0.0 ) 
&& (normaliz(alpha2 - alphal) > 0.0) 
|| ((bearingl - bearing2 < 0.0) 
&& (normaliz(alphal - alpha2) > 0.0)))) 


рег->ргеу - templ; 
templ-»next - ptr; 
ptr-»next - NULL; 
world[n].centroidx += (ptr->headx + ptr->tailx); 
world[n].centroidy += (ptr->heady + ptr->taily); 
world[n].seg_count += 1; 
tril = triangle_area(world[n] .headx,world[n] .heady, 

world[n]l-tarlxworloin]. tout 

ptr->headx, ptr->heady) ; 
tri2 - triangle area(world[n].headx,world[n].heady, 

ptr-»headx,ptr-»heady,ptr-»tailx,ptr-»taily); 
world[n].area += (tril + tri2); 
world[(n].tailx = ptr->tailx; 
могла [сату = ptr—--taily- 

} 
else if((abs (quadrant (bearingl) - quadrant (bearing2)) == 3) && 
((bearing2 - bearingl > 0.0 ) 
&& (normaliz(alpha2 - alphal) > 0.0) 
|| ((bearing2 - bearingl < 0.0) 
&& (normaliz(alphal - alpha2) > 0.0)))) 


ptr->prev = templ; 

templ->next = ptr; 

ptr->next = NULL; 

world[n].centroidx += (ptr->headx + ptr->tailx); 

world[n].centroidy += (ptr->heady + ptr->taily); 

world[n].seg count += 1; 

tril - triangle area(world[n].headx,world[n].heady, 
мота. саз моста Саз 
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ptr-»headx,ptr-»heady); 
tri2 - triangle area(world[n].headx,world[n].heady, 
ptr->headx, ptr—>heady, ptr->tailx,ptr->taily) ; 
world{n].area += (tril + tri2); 
world[{n].tailx ptr->tailx; 
world{n].taily per (ај. 157“ 
) 
else 


( 


end. poly(ptr,n); 
poly num += 1; 
init next poly(ptr,poly num); 


f Ck ke e ke ne ke ke e e ee e e e e e KOK KK KOK KK AK AKK KK KAKAK A e ce cese e ck ke ce ce ke e ck sk ke ke ck ck ke ck kk kk kk ko ko kc ko ko 


FUNCTION: print list) 

АОТНОВ: Mike Campbell 

DATE: 4 March 1996 

PURPOSE: This procedure outputs the final results for further 
evalutation. 

RETURNS: None. 


ck ckockockck oko eee cse ck oe e koe e e oe se oe e e ke he e e oe e e e e ke e oe ee e e e e Se oe e ee e e e e e e se ke ck ke ke de КСК К 


о prine оС.) 


Struct LINE SEG *temp ptr; 


for (i = 0; i < poly_num + 1; i++) 
{ 
if (world[i].head of poly -- NULL) 
temp_ptr = NULL; 


else 

temp ptr - world[il.head of poly-»next; 
while (temp ptr !- NULL) 
( 


fprintf(objectfile,"$4g $4g %49\п%49 $4g $4g $4gWMn",temp ptr-»headx, 
8l 


temp ptr-»heady,temp ptr-»orientation, 
temp ptr-»-tailx,temp ptr-»taily, 
temp ptr-»alpha,temp ptr-»r); 
temp ptr - temp ptr--»next; 
) 
Еркап  Е(орјесе и ен 15 
/хкккхкккиккик THIS section commented to гасі: Басе рое а с Na 


fprintf(outfile, "Average centroidx is: %4g Nn”, 
(world[i].centroidx / (world[i].seq_count™ * 729) | 
fprintf(outfile, "Average centroidy is: %4g Ап", 
(world[i].centroidy / (world|/i].seq count’ *—2))))- 
fprintf(outfile, "Area is: %4g \n",world[i].area) ; 
fprintf (outfile, "Classification is: $i WMn",world[i].classification); 
fprintf (outfile, ПАХ ТА) ТІ МУ) ЖК ЖКААЖ ЖАТА I 
fprintf(new world,"$f ФЕ ФЕ ФЕ ФЕ ФЕ ФЕ ФЕ ФЕ %1 Zi $iWMn", 
world[i].start,world[i].end,world[i].headx, 
world[i].heady,world[i].tailx,world[i].taily, 
world[i].centroidx,world[i].centroidy,world[i].area, 
world[i].seg count,world[i]l.status, 
world[i].classification) ; 


/*fprintf (outfile, "There are $21 objects \n“,pely num += 1); *7 


/*This is sonar globals.h */ 


FILE *worldfile,*new world, *textfile,*outfile,*out2,*objectfile; 
char line[132]; 
Tnt та ао. 


st path,Rotate count,Rotate status,poly num - 0, бопаг_поае=-1, 

one bad return,Search status,bin, threshold,LOCATIONLAB = 0; 
double ti, t2, Ба, td; t5) tb. ЕР, Оо, СЕ м 

x search,y search,SONAR 725 range,SONAR 725 bearing; 


double step size - 1.0, SONAR 725 strength, sigma weighting - 3.0, 
residual tolerance - 2.0, thinness requirement -z0.1, x return, 
v return: 

double ота х ота у 2 global radius = 1; 

ine Scan_direction = -1; /* -1 is cw scan 1 is for ccw scan */ 


typedef struct { 
double theta; 
double r; 
int nun ponts; 
int line_status; 
double initx[4]; 
double inity[4]; 
double sgm delta sq; 
double start time; 
double startx; 
double starty; 
double endx; 
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double 
double 
double 
double 
double 
double 
double 
double 


endy; 
SgImnx ; 
sgmy ; 
Sgmx2; 
sgmy2; 
Sgmxy ; 

d minor; 
d major; 


) SEG DAT; 


Seruct LINE SEG 


{ 


} 


double 


double 


double 


double 


double 


double 


struct 
ОЕ ТИСЕ 


. 
, 


start time, 
finish, time; 
headx, 

heady; 

вах, 

taily; 

alpha, 
orientation, 

E; 

length; 

dmajor, 

dminor; 

LINE SEG *next; 
LINE SEG *prev; 


struct Polyhedron 


( 


Je 


Sermuct 


double 
double 
спа (еле 
double 


double 
double 


double 
double 
avt: 
IE 
int 


SEG_DAT 


СЕЕ ИСЕ 
Struct 


start_time; 
end_time; 
LINE_SEG 
faa lx, 
taily; 
ћеадх, 
ћеаду; 
centroidx, 
centroidy; 
area; 
alpha;/*for walls only*/ 
seg count; 

status; 
Classification; 


*head of poly; 


/Х1-ма11, 


LINE SEG *head 
segment data; 
INE SEG *finished segment; 


Polyhedron world[100]; 


NULL; 
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Cepu аза пе l complete */ 
3=unknown* / 


84 





APPENDIX B. SOURCE CODE FOR SONAR COMMUNICATIONS 


а о Ро и ои о КИМИ НИТИ И НОСИ ХК ok e o Kok ok koe oe e kc e e kc e ke ek kk 


FILENAME : Sonar comms.c 


AUTHOR: Mike Campbell 
Sonar Communication Code: Modified from Dave Marco's Code 
Serial Port Initialization Code:Modified from Dave 

McClarin's Code 

DATE: 22 January 1996 

PURPOSE: Handle all communications with the sonar system including: 
initialization and pinging sonar. 

REVISION: This code 15 constantly being improved and expanded current 
revision is available at: 
http://www.stl.nps.navy.mil/~auv/tactical/ 

FUNCTIONS: int initialize sonar. serial(); 


А о А ааа оаа А 


#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 


int initialize sonar systems(); 
double Ping Sonar(); 

void initialize sonar(); 

char set scanning gain(); 

void center. sonar(); 

char send command(); 

Mite read porti): 


oe Gon > 
<math.h> 
<stdlib.h> 
<signal.h> 
<sys/types.h> 
“Tents. i> 
<ctype.h> 
cerrno.h» 
<string.h> 
«stropts.h» 
«sys/conf.h» 
«sys/stat.h» 
<unistd.h> 
"sonar globals.h" 
"termiox.h" 
«sys/uio.h» 
«termio.h» 


EILE *ouüutfp; 


double sonar_heading; 

int initialize, sonar. serial(); 
int initialize sonar_systems(); 
double Ping Sonar(); 

мота rnitlalize-sonar (); 

char set scanning gain(); 

void center sonar(); 

char send command(); 
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ine read port), 


• 
/ ХАК ЖЖ АСА АС ААА АСА қ кк кк т ата та a ЭХ 


FUNCTION: initialize sonar. systems() 
AUTHOR: Mike Campbell 
PURPOSE: Initializes the system including the serial port and 


sonar head. 


RETURNS: One if 1t 15 successful с/о Еи е 
KANA KK KK KIK KAKAK AK KK KK IKI KIK ккк КА КАКА KU Ic oU oec ck ek x Ex UNA А E RUE ны а ааа 


int initialize. sonar. systems() 


char c[1],x1(50],x2[150] 55 [50] 936 (2008 a E 

chere b 

unsigned short bin byte,dummy byte,binO,binl,bin2,b1in3; 
char buffer[100]; 

int i,j,k,n,w path,r, path,n loops,n bytes,nnn; 

int RESET PORT,timeout; 


bial ay ne eee) 
char st725_mode; /* "5" for Scanning, 'P' for Profilingk m 
int st725 NBINS; /* No. of bins to collect 64 or 12594] 


int st725_max_range; 
int st725_power; 
int st725_ Ecpuls; МАЛ 
int st725 psi sonar count: 
double st725_psi_sonar; 
int st725_sweep_sign; 
НЕЕ. 
if ((outfp = fopen ("raw даба sonar", "м“)) == (БРЕ. | о) 
( 

printf("Error opening Out File iniy. 

exit(0); 
} 
st725 mode 
SET25 pss-sonar-count - 
22725 рез гонат 
St725 sweep sign 
St725 59517 = 
sSt725 max range 
st725 power 


! 
п 


H 
== се сну 
о 


tt 
Ол 


152% 
r path - initialize sonar serial("/dev/ttya"); 


initialize sonar(r. path,st725, mode,st725 max range,st725 power, 
&st725 Nbins,st725. ssiZ,St725WMBeps bs). 

center sonar(r. path); 

sonar heading - 0.0; 

return r path; 
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К К ^^. K KK KK KA KAKAK KAKAK KAKAK AKK AK KAKAK KAN KAKAK AKK AKK 


FUNCTION: Ping Sonar. () 
AUTHOR: Mike Campbell 
PURPOSE: Pings the sonar and scans in the Scan direction, reads 


RETURNS: 


the sonar response and calculates Range ignoring first 
bin threshold bins 


Returns range in feet and sets SONAR 725 strength 


MEL LLL uc КАБАККА KEE ERE REE ERREKRKKKKEKKEEKKKE / 


double Ping Sonar (r. path,Scan direction,bin threshold) 


int r. path,Scan direction,bin threshold; 


| 
enar <= [1], [200],У[1]; 
int Intensity[64],i,max range -» 0,пах loc = 0; 
cham ХЕ; 


unsTgned short bin byte,binO,bini,bin2,bin3; 
int k,n,num bytes; 
double range; 


switch(Scan direction) 


( 
case -1: 
elol = в’: 
sonar_heading += 0.9; 
break; 
case 0: 
elol = "5"; 
break; 
case i1: 
elol = Маш; 
sonar heading -- 0.9; 
break; 
) 


write(r path,c,1); /* write characters to path /t2 dev. 


іосЁ1 (їг path,I NREAD,&num bytes); 
for (k=0;k<16;++k) /* Read First 8 Bytes */ 
{ 
t= Abin byte; 
n-read(r path,x,1); 
tlo] [0]: 
n=read(r_path,x,1); 
eL - 2 ОД; 


Bing 
bin3 


ban byte << 12; 
Бата >> 12: 
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65) 


Бїп2 s bin bytea в. 
Раоа Din? ME 


bini = “bin 5D EET. 
bial = bino rr 


Dino = bin byte 6 


Intensity[k*4] = bind; 


Intensity[k*4 + 1] = binl; 
Intensity[k*4 + 2] = bin2; 
Intensity[k*4 + 3] = bin3; 


LE ЕО) 


n=read(r_path,x,1); 
for(i = Din threshold; 1 < 64: т.ғ) 
{ if(Intensity[1] > max_range + 2 && Intensity[i] > 7) 
{ 
max range = Intensity[1i]; 
max 106 = 1; 
) 
) 
if(max loc » 0) 
| 
range = 0.5126%(1 ж пах loc); /*conversion from bin # to range in feet 
set for 10 meter scale*/ 
SONAR 725 strength 


max range; 


else 
( 
range - 0; 
SONAR 725 strength = 0; 
) 


return range; 


) 


f KR ke e e che e che eoe ke e e KR KKK KR e ke ke ke ke ce ce ce ck ke ce ce ke ke ke ce ke he ce ce ck ke ke ke ce ck ce KR KR ke ce ke ce Se ce ce ke ce KR ke ke ce ce ke e e ke Жо Ж 


FUNCTION: initialize. sonar serial() 

AUTHOR: Mike Campbell Modified from Code of Dave McClarin 
PURPOSE: Initializes serial port /dev/ttya 

RETURNS: O if failed and 1 if succeeds 


К А КОЕ КЗ kp XE USt НАРАВ ован 


int initialize sonar serial(char port[50]) 
( 
че а. рас, = ва 


struct termio term; 


if ((path = open(port, O_RDWR|O_NONBLOCK)) == -1) 
{ 


forintf (stderr ,"No serial’ connecwion. ua. 
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else 
{ 
path = open(port, O_RDWR) ; 
memset (&term, 0, sizeof(term)); 


term c iflag = IXOFF; 

term.c_oflag = 0; 

term.c_cflag = B9600|CS8|CLOCAL|CREAD|HUPCL; 
term.c lflag s 0; 


term.c line = 0; 
toerm.c cc[VMIN] = 0; 
perm.c-ce[VTIME] - 1; 
ВЕС roctliípath, TCSETAFE, &term) == -1) 
fprintf(stderr,"Unable to set port parameters.\n"); 
} 


return path; 


} 


Cee EK KEEEKEKKKEREEKE KE EEKREREREEKEKEKEKEKREKREEKEEKKEKKKEKEKEKEKEKKEKKEKKKKKKKKE € 


FUNCTION: read port() 

AUTHOR: Mike Campbell Modified from Code of Dave Marco 
PURPOSE: Reads the serial port 

RETURNS: Number of bytes read 


КАКА Ххх кх у eoe o lee eoe ve oe e e o oe fe e ok ce e sk dece e de e sede de e eoe de e oe de he he de de he de ke he nhe he e e de e ok n / 


int read port(int port, fd, int num bytes, char *data) 


( 
const int MAX RETRIES - 100; 
ЕНЕ СОППС -0- 
int num tries = 0; 
imc Tbr: 
while ((count < num_bytes) && 
(num_tries < MAX_RETRIES) && 
((nbr - read(port fd, &data[count], 1)) != -1)) ( 
num tries-«-; 
count += nbr; 
) 
iR Count '= mum bytes) 4 
if (num tries -- MAX RETRIES) 
fprintf(stderr, "Error: too many retries reading serial port\n"); 
else 
fprintf(stderr, "Error: serial port read failed\n"); 
раша (stderr Min read port \n"); 
} 
EC IS mnecourt; 
) 


АКК oe ook oe sese eK ok e ok ok eoe e kk CK KK KO kk ok Koo 
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FUNCTION: send_command () 


AUTHOR: Mike Campbell Modified from Code of Dave Marco 
PURPOSE: This function sends a single character and reads back a 
single 


character from the sonar 
RETURNS: char read from sonar 


X koe eee Se e ke KK Sek e ee he she e e ok SKK OK KU eco ok Ye ж ж X Ok ЮЖО жж лк та а 


char send command(path,command) 
int path; 
char command; 


unsigned n,n_bytes=0; 
char reply,x[20])],c[1]; 
c[0] = command; 


п = write(path. C, 1); 


while(n_bytes != 1) 
ioctl(path,I. NREAD,&n, bytes); 
if(n bytes == 1) 

( 


п = read(path,x,n, bytes); 
) 
reply = x[0]; 
return (reply); 


УЖ ЖКХ ххх ххх хк хк к кж ж ккк к хх ЖКК ат EL ETE 


FUNCTION: initialize_sonar () 

AUTHOR: Mike Campbell Modified from Code of Dave Marco 
PURPOSE: Initializes sonar head to desired parameters 
RETURNS : None 


Wc eee See ee ee e eee he КСО КУКИ И АЖК КИИ АККА ИККИ КК КИ И 


void initialize_sonar (path,mode,max_range,gain,nbins,ssiz,Ecpuls) 


int path; /* Path opened for particular head */ 

char mode; /* 'S' s Scanning mode, 'P' - Profiling mode */ 

int max range; /* 1, 2, 4, 6, 10, 20, 25, 30, 50, ок 100 тегек ви 
int gain; /* 0 <= gain <= 100 / 

int *nbins; /* # of bins to collect 64 or 128m7 

ine 5-17, т, 2, от = ОТО 133, отео гожа аи“ 7 


ints Hepuls= 


unsigned n_bytes; 
unsigned short T1,T2,T3,Tchecksum; 
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Ки ава са је tyl] reply; 

unsigned short byte,bytel,Nsampl,Nbins, Range_Code, checksum; 
unsigned short TxPulseMSByte, TxPulseLSByte,Gecmin, Rng_unt; 
int i,j,k,n,word,TxPulse; 


int Timout,Lokout,Eswait,Gaindt,Ecsclx,Ecscly; 
int Maxdst,Dacscx,Dacscy; 
int EchoSounder[10]; 


EE OEOEUKORUNGREOK И he he e e e e de e he e he e e хх} К Ж ek 


INITIALIZATIN PARAMETERS FOR SCANNING MODE (ST-725 AND ST-1000 HEADS) 


Range | TxPulse | NSAMPL | NBINS | Range Code | checksum 
meters | 1.96 usec | | | | Lower 8 bits of 
sum 
| dec hex | dec hex | dec hex | dec hex | dec hex 
6 | 30 ШІЕ| ПІ 01) 64 мој 0 Ш | 95 5Е 
10 8 001Е | Оз | 62/0 | 1 ШІ | 5545) 
20 | 100 0064 | B а 28 80 | P 02 | 233 Е9 
25 КОО ШОО ор "ОООО 07 | 15 80| 3 03 | 4 04 
30 5077 0096 | Spoon) 128. 80 | 4 ол 000001 
50 БЕС (БА) 12 сес | 128 980 | 5 05 | 139 ав 
100 ШОЛ ШЕЕ | 265 “1А | 128 80 | Г 07 [2600575 


хкккккикккккиккиккккккккккккккккккккккккккккккккккккккккккккккккккккккккккккккккккк Ж 


INITIALIZATIN PARAMETERS FOR PROFILING MODE (ST-1000 HEAD) 


Range | TxPulse | NSAMPL | NBINS | Range Code | TIMOUT | Maxdst 
meters | | | | | | 
| | | | | | 

1 | 30 | П | 64 | 00 ESOO | 1500 
2 | 30 | 1 | 64 | 01 | 3000 | 3000 
4 | 30 | 1 ЕЕЕ | 02 | 6000 | 6000 
6 | 30 | I 712817 03 | 9000 | 9000 
10 | 40 | jl |“ 128. | 04 | 15000 | 15000 
20 | 50 | 3 1287 | 05 | 30000 | 30000 
30 | Te | 6 MERE 501 06 | 45000 | 45000 
50 | 100 П 2 Т 07 МИНОВ 655365 
ECPULS = 30 
EOKOUTP = 200 

ESWAIT = 25600 
GECMIN = Byte 
GAINDT = 64 

ECSCLX = 16383 

око (е = T1374 
DACSCX = 256 
DACSCY = 3125 
Pag Unt = l 

kkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkk/ 

if (mode == 'S') /* Set up for Scanning */ 

( 

switch(max range) 
( 
case 6: 
TxPulse = 30; 
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Nsampl 

Nbins 

Range, Code 

ЧЕ 

m» 

T3 

Tchecksum 
break; 


case 10: 
TxPulse 
Nsampl 
Nbins 
Range Code 
ТЇ 
T2 
ШЗ 
Tchecksum 

break; 


case 20: 
TxPulse 
Nsampl 
Nbins 
Range Code 
TI 
T2 
T3 
Tchecksum 

break; 


case 25: 
TxPulse 
Nsampl 
Nbins 
Range_Code 
TI 
T2 
T3 
Tchecksum 

break; 


case 30: 
TxPulse 
Nsampl 
Nbins 
Range Code 
ШЕТ: 

T2 

T3 

Tchecksum 
break; 


case 50: 


64; 
0; 
or 

65: 

2 

80; 


= 30; 


Зе 
64: 


В; 
65; 
20; 
88; 


= 100. 


oe 


128; 
2 
3 


1297 
40; 
gro 


2925. 


128; 
3; 
oF 

129; 

БО; 


182; 


125102» 
б; 


128; 
4; 


36 


1:297 


60; 


1927 
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TxPulse 

Nsampl 

Nbins 

Range Code 

TA 

T2 

P3 

Tchecksum 
break; 


case 100: 
TxPulse 
Nsampl 
Nbins 
Range Code 
TI 
ДЫЎ, 

T3 
Tchecksum 
break; 


= 925906 


26 
12897 
9; 
3) 5 
1297; 
100; 
2327 


= 475; 


~ Епа Switch */ 


) 
else if (mode == 


{ 


switch(max range) 


( 
case 1: 
TxPulse 
Nsampl 
Nbins 
Range Code 


атпоо С 
Maxdst 
break; 


case 2: 
TxPulse 
Nsampl 
Nbins 
Range Code 


Timout 
Maxdst 
break; 


case 4: 
TxPulse 
Nsampl 
Nbins 
Range Code 


I3mobpit 
Maxdst 


'P') 


26: 
286 


3 
#20. 
200; 
16: 


O Ser up for Profiling */ 


= 0: 


qe 
64; 
О; 


SOOS 
1500; 


= 30; 


157; 
64; 
E 


= 3000; 


3000; 


mo 
l; 
128; 
2; 


6090; 
6000; 
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ркеак; 


сазе 6: 
TxPulse = 30; 
Nsampl L; 
Nbins = 128; 
Капае Соде = 3; 
Timout = 9000; 
Maxdst = 9000; 

break; 

case 10: 
TxPulse = 40; 
Nsampl = Д8; 
Nbins — 120; 
Range_Code = 4; 
Timout = 19000; 
Maxdst = 15000; 

break; 

case 20: 
TxPulse = 50; 
Nsampl 33 
Nbins = 1286: 
Капае Соде = S4 
Timout = 30000; 
Maxdst = 30000; 

break; 

case 30: 
TxPulse = 75; 
Nsampl 6; 
Nbins = 128; 
Вапае Соде = бе; 
Па ај и = А5000; 
Maxdst = 45000; 

break; 

case 50: 
TxPulse = 100; 
Nsampl = 12; 
Nbins = is. 
Range_Code = 7: 
Timout =65535; 
Maxdst -76525355 

break; 


] 7*—End switch *J/ 


/* Values Common to all Ranges */ 
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ери 5 = 75; % 

Portier input EcpulsNn"): 
scanf("%d",&Ecpuls) ; 

PENGENE NG); 

ЕСО = кахп ,„Есри15); 
ВЕНЕ ки) t7 


Lokout = 200; 
Eswait = 256007 
Gecmin ND аап; 


рас Е ("иту 
ЕТЕ Света = ако" Сеспап) ; 
ШЕЕ (РУ 


Gaindt ЕСА 
Ес5с1х = 16383; 
Ecscly = 11374; 
Dacscx = 256; 
Dacscy ИИ... 
Rog unt = 

) 

else 


( 
printf("Wrong mode! \n") ; 
} 


*nbins = Nbins; 


/* Send Sonar Parameters */ 
СОГ = р" 
wrrte(path,c,1l); 


/* Send TxPulse Length (Word) in 1.96 usec units */ 
word = TxPulse & Ox00ff; /* Byte - LSByte of TxPulse */ 
byte - word; | 

TxPulseLSByte - byte; 


y[0] = (char) byte; 
nysswrrte(path.v,l); /* Send LSByte First */ 
word - TxPulse »» 8; /* Byte - MSByte of TxPulse */ 


byte - word; 

TxPulseMSByte - byte; 

y[0] = (char) byte; 

n -owrrte(path,v,1); /* Send MSByte Last */ 


/* Send NSAMPL (Byte) NO. A/D Samples per Bin */ 
byte = Nsampl; 

УГО] (char) byte; 

n - write(path,y,1); 


/* Send NBINS (Byte) No. of Bins to Collect */ 
byte = Nbins; 

y[0] » (char) byte; 

n - write(path,y,1); 
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/* Send Range Code 0-8 (obsolete) (Byte) */ 

byte - Range. Code; 

y[0] = (char) byte; y 
n  write(pathy 1); 


/* Send DataByte checksum (Byte). Should be the lowest 8 bits of */ 

К the sum of all Bytes */ 

word = TxPulseMSByte + TxPulseLSByte + Nsampl + Nbins + Range. Code; 

word = word & Ox00ff; /* Mask MSByte to get last 8 bits for checksum; */ 
checksum = word; 
byte = checksum; 

у[0] = (char) byte; 

п = write (pach y. l); 


sleep (1); 
1octl (path, I_NREAD, &n_bytes) ; 
/* Read Reply to Checksum */ 
n = read(path,x,1); 
теру = ГО; 
{теру = та) 
{ 
printf("Parameter Checksum Ok\n"); 
} 
else 
{ 
printf ("Parameter Checksum INCORRECT! !!\n"); 


/* Enable halfstep should reply 'Н' */ 
reply - send command(path,'H'); 
if (reply == н”) 
{ 
printf("Half step set\n"); 
} 
else 
{ 
printf("Half step not set!WMn"); 


/* Enable TVG should reply X +7 
reply = send_command (path, 'X'); 
че (тер sz 5) 
{ 
РЕТИЕЕ( ГОА СЕ\ и": 
} 
else 
( 
рїп ("ТУС пок вес ај 


/* Set mode return Range bin Peak should reply 'K' */ 
reply = send. command(path, 'K'); | 
етеру ==) 
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printf("Range bin РеаК тоде ОКАП"); 
) 
else 
( 


printf ("Range bin Peak mode not set!\n"); 


/* Set Final Gain for TVG, should reply 'Е' */ 
reply - set scanning gain(path,83,'E'); 
Hi (reply == 'E') 
{ 
printf("Final TVG Gain set\n"); 
) 
else 
( | 
printf("Final TVG Gain not set!\n"); 


if (mode == 'S') /* Scanning mode */ 
i 
y= Set Initial Gain for TVG, should reply 'C' */ 
reply = set_scanning_gain(path,gain,'C'); 
к терү == ) 
{ | 
ПОЛИ ЕЦ алата TVG Gain setWin"); 
) 
else 
( 
printf("Initial TVG Gain not set!\n"); 


} 
else /* Profiling mode */ 


{ 


EchoSounder[0] = Ecpuls; 
EchoSounder[1] = Timout; 
EchoSounder[2] = Lokout; 
EchoSounder[3] = Eswait; 
EchoSounder[4] = Gaindt; 
EchoSounder[5] = Ecsclx; 


EchoSounder [6] = Ecscly; 
EchoSounder[7] = Maxdst; 


EchoSounder[8] = Dacscx; 
EchoSounder[9] = Dacscy; 
checksum = 0; 


/* Send Profiler Sonar Parameters */ 


SiO t. 
write (path,c, ll); 


/* Send First 4 Parameters (Words) */ 


o7 


О (== (рате Ан 4-1) 


( 


) 


word - EchoSounder[i] & Ox00ff; /* Byte = LSByte of EchoSounder{i] */ 
byte = word; ш 

checksum = checksum + byte; /* Add up the checksum */ 

у[0] = (char) byte; 

n = write (path,y, 1); /* Send LSByte First */ 


word = EchoSounder [1] +> 8; /* Byte = MSByte of EchoSounder[i] */ 
byte = word; 

checksum = checksum + byte; /* Add up the checksum */ 

y[0] » (char) byte; 

п = мае (рае. Yy, L); /* Send MSByte Last */ 


/* Send Gecmin (Byte) */ 
byte - Gecmin; 

checksum = checksum + byte; 
у О = (char) pyte; 


п 


write (path,y,1); 


/* Send Last 6 Parameters (Words) */ 
Рог (1=4;1<10;++1) 


{ 


} 


мока EchoSounder[i] & OxOOff; /* Byte = LSByte of EchoSounder[i] */ 
byte - word; 

checksum = checksum + byte; /* Add up the checksum */ 

y[0] = (char) byte; 

n = гасе (равии /* Send LSByte First */ 


word = EchoSounder[i] >> 8; /* Byte = MSByte of EchoSounder[i] */ 
byte = word; 

checksum = checksum + byte; /* Add up the checksum */ 

УГО] = (char) byte; 

n = writelpath,y, 1); /* Send MSByte Last */ 


/* Send Rng unt (Byte) */ 
byte - Rng unt; 

checksum = checksum + byte; 
y[0] 7» (char) byte; 


n 


write(path,y,1); 


/* Send DataByte checksum (Byte). Should be the lowest 8 bits of  */ 


Jak 


the sum of all Bytes */ 


checksum = checksum & 0х00ЕЕ; /* Mask MSByte to get last 8 bits */ 


printf("Profile checksum = %d\n", checksum): 
byte = checksum; 
У[0] = (char) byte; 


n 


write(path,y,1); 


sleep(1); 


ioctl(path,I. NREAD,&n bytes); 


/* Read Reply to Checksum */ 
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п = read (path,x, 1); 
reply = x[0]; 


if(reply == 'Т') 
{ 
printf("Profile Checksum Ok\n"); 
} 
else 
{ 
printf ("Profile Checksum Incorrect \n" ) > 
} 


} 

/* Check if head is using default settings. Reply is 'T' if yes, */ 
Те“ af not */ 

reply = send_command(path,'D'); 


Егеру == 'T') 
{ 
printf("Head is still using Default Settings!\n"); 
} 
else 
{ 
printf("Head not using Default Settings\n"); 
} 
} 
re ERE EERE REE EEE EERE oko 
FUNCTION: center_sonar () 
AUTHOR: Dave Marco 
PURPOSE: Function to set sonar gain, 0 <= gain <= 100 
FETURNS: none 


AE aE ee AAAA AAA AAA AAAA A le ee eoe Se oe coe e eoe e UC Gk o x x 7 


char set scanning gain(path,gain,which gain) 


Tnt path, gain; 
МЕН wareh gain: /* which gain = "B' for Initial, 'E' for Final */ 
{ 

unsigned short byte; 

unsigned n,n_bytes; 

Ghar reply лІі|,х|201,с<(1); 


/* Set Initial or Final Gain for TVG should reply */ 
Же ЕБ анта! or Е" for Final */ 

с[0] = which gain; 

write(path,c,1); 

byte = 2.55*gain; 


y[0] = (char) byte; 
п = write(path,y,1); 
ѕ1еер (1); 


ioctl(path,I NREAD,&n bytes); 
read(path,x,n bytes); 

reply - x[0]; 

return(reply) ; 
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ЕЖЕН ЖЕНКИ ЖАК И ЖЕ ЖАК КИ КА АНА АКАТА АИ АТА ТА А АИ ы 


FUNCTION: center sonar() 

AUTHOR: Dave Marco 

PURPOSE: centers the sonar head 
RETURNS: none 


Жаа ЖА л ллы „л к.к к. ы к.җ йг а тата I c 


void center_sonar (path) 
int path; 


in a 
int direction, encoder width; 
char encode; 


encode='A'; 

encoder width - 0; 

direction = 1; 

printf("Inside center\n"); 

/* Clear out any junk from buffer at startup */ 

while(encode != '3') 

{ 
printf("encode = %c path = %d\n", encode, path) ; 
encode = send_command(path,'V'); 


/* Are we inside the Encoder Sensor ? */ 


encode = send_command(path,'M'); /* Test Head Direction (No Step) */ 
if((encode == 't') || (encode == 'Т')) 
{ 
while( (encode == 't') || (encode == 'T') ) 
{ 
encode = send_command(path,'+'); /* Index Sonar '+' direction */ 
} 
/* Outside Encoder Sensor Now */ 
direction = -1; /* Reverse Sonar Rotation to Establish Encoder Width */ 
} 
while( (encode == 'f') || (encode == 'Е') ) 
{ 
if (direc ion == 1.) 
{ 
encode = send_command(path,'+'); /* Index Sonar '+' direction */ 
} 
else 
{ 
encode - send command(path,'-'); /* Index Sonar '-' direction */ 


) 
100 


} 
/* Found Edge of Encoder */ 


while( (encode == 't') || (encode == 'T') ) 
( 
encoder width - encoder width + 1; 
ir dimection == I) 
{ 
encode = send_command(path,'+'); /* Index Sonar '+' direction */ 
} 
else 
{ 
encode = send_command(path,'-'); /* Index Sonar '-' direction */ 
} 


/* If direction = +1, Go Back 5 Steps to Establish Center 
If direction = -1, Go Forward 5 Steps to Establish Center */ 


D direction == 1) 
{ 
Гот(1=0;1<5;++1) 
{ 


encode = send command(path,'-'); 


) 

"E(drrection zz -1) 

( 
for(1=0;1<5;++1) 
{ 


encode - send command(path,'-'); 
} 


printf("Center EstablishedWMn"); 
sonar heading = 0.0; 
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APPENDIX C. CODE FOR SONAR GNUPLOTS 
HAHAHAHA AH Ha a aH oH PE aH aR PER A PR RH RAR RHA EH EH 


# # 
# filename: excerpt from auv plot 1 second.gnu # 
Я # 
# function: GNUPLOT V3.5 script to plot AUV telemetry data # 
1 to screen & to PostScript files # 
# # 
я updated: 12 March 96 i 
Ќ # 
# author: Don Brutzman, excerpt written by Mike Campbell # 
# # 
4 execution:  gnuplot» load "auv. plot 1 second.gnu" # 
# gnuplot» reread Я 
# # 
# unix> gnuplot auv_plot_l_second.gnu + 
# # 
К re-plotting: # 
# 'Xpsview' -wp -skipc -or landscape ~/execution/AUV_telemetry.ps & # 
# ghostview -landscape -/execution/AUV telemetry.ps & # 
я # 
# С program call: system ("gnuplot auv plot 1 second.gnu"); ü 
# # 
# telemetry: mission.output.telemetry (AUV telemetry 0.1 sec interval) # 
# alternate:  mission.output.1 second (AUV telemetry 1.0 sec interval) # 
# # 
# original plot: unix» gnuplot  auv plot.gnu # 
# # 
# output files:  AUV telemetry.ps & *.eps plots я 
# # 
# | related files:  execution.c # 
# underwater virtual world қ 
# # 
# output archive:  ftp://taurus.cs.nps.navy.mil/pub/auv/AUV telemetry.ps.Z # 
# # 
# gnuplot FAQ: ftp://ftp.dartmouth.edu/pub/gnuplot/faq/gpt faq.html 4 
# 
# GS Er DUE ION: http://www.cs.dartmouth.edu/gnuplot/ # 
# # 


EEE HE EE HE HE EE E HE EE HE HE HE HE HE E HE HE E HE HE HE HE HE E HE HE HE EE HE HE HE HE FE HE HE HE HE E HE HE HE HE HE HE HE HE HE HE HE HE HE HE FE HE FE FE E HE HE FE HE E HE HE HE HE HE EE E EE E HH HH 


# setup: 
set terminal x11 # gnuplot version 3.4 (no auto redraw) 
# set terminal iris4d Ngmuplobsversion 3.5 (x11 is OK) 
set time 
set grid 


set data style linespoints 


set samples 10 # data point plotting frequency 
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ННЕИННИННИНИНИНИНИНИНИИИЕИНИНИНИНИНИНИННИИНИНИНИНнИНИНИНИНнИНИНЕ ЕЕНИНИНИННННИНИН 


set xlabel "East -» (y world) [ft]" z 
set ylabel "North ^ (x world) [ft]" 


pause -1 "hit enter to continue with sonar plots" 
# ааа sonar here 


set title  "NPS AUV sonar outputs" 26,.8 

plot "../tactical/data_points.sonar" title "processed sonar returns" with 
points 

pause -1 "hit enter to continue with sonar plot 2 " 


set title  "NPS AUV sonar outputs" 26,.8 
plot "../tactical/line segments.sonar" using 2:3 title "fitted line segments" 
pause -1 "hit enter to continue with sonar plot 3 " 


set title "NPS AUV sonar outputs" 26,.8 
plot "../tactical/data_points.sonar" with points, \ 
"../tactical/line segments.sonar" using 2:3 title "line segments over 
data pointe" | 
pause -1 "hit enter to continue with sonar plot 4 " 


set title  "NPS AUV sonar outputs" 26,.8 

plot "../tactical/line segments.sonar" using 2:3, "../tactical/objects.sonar" 
title "objects imposed over line segments" 

pause -1 "hit enter to continue with sonar plot 5 " 


set title "NPS AUV sonar outputs" 26,.8 
plot "../tactical/objects.sonar" title "objects built from sonar data" 
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